public virtual void collideEdgeAndPolygon(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB)
{
collider.collide(manifold, edgeA, xfA, polygonB, xfB);
}
/// <summary>
/// Get a child edge.
/// </summary>
public void GetChildEdge(EdgeShape edge, int index)
{
Debug.Assert(0 <= index && index < Count - 1);
edge.Radius = Radius;
edge.Vertex1.Set(Vertices[index + 0]);
edge.Vertex2.Set(Vertices[index + 1]);
if (index > 0)
{
edge.Vertex0.Set(Vertices[index - 1]);
edge.HasVertex0 = true;
}
else
{
edge.Vertex0.Set(m_prevVertex);
edge.HasVertex0 = HasPrevVertex;
}
if (index < Count - 2)
{
edge.Vertex3.Set(Vertices[index + 2]);
edge.HasVertex3 = true;
}
else
{
edge.Vertex3.Set(m_nextVertex);
edge.HasVertex3 = HasNextVertex;
}
}
public virtual void collide(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB)
{
Transform.mulTransToOutUnsafe(xfA, xfB, m_xf);
Transform.mulToOutUnsafe(m_xf, polygonB.m_centroid, m_centroidB);
m_v0 = edgeA.m_vertex0;
m_v1 = edgeA.m_vertex1;
m_v2 = edgeA.m_vertex2;
m_v3 = edgeA.m_vertex3;
bool hasVertex0 = edgeA.m_hasVertex0;
bool hasVertex3 = edgeA.m_hasVertex3;
edge1.set_Renamed(m_v2).subLocal(m_v1);
edge1.normalize();
m_normal1.set_Renamed(edge1.y, -edge1.x);
float offset1 = Vec2.dot(m_normal1, temp.set_Renamed(m_centroidB).subLocal(m_v1));
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
edge0.set_Renamed(m_v1).subLocal(m_v0);
edge0.normalize();
m_normal0.set_Renamed(edge0.y, -edge0.x);
convex1 = Vec2.cross(edge0, edge1) >= 0.0f;
offset0 = Vec2.dot(m_normal0, temp.set_Renamed(m_centroidB).subLocal(m_v0));
}
// Is there a following edge?
if (hasVertex3)
{
edge2.set_Renamed(m_v3).subLocal(m_v2);
edge2.normalize();
m_normal2.set_Renamed(edge2.y, -edge2.x);
convex2 = Vec2.cross(edge1, edge2) > 0.0f;
offset2 = Vec2.dot(m_normal2, temp.set_Renamed(m_centroidB).subLocal(m_v2));
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal0);
m_upperLimit.set_Renamed(m_normal2);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal0);
m_upperLimit.set_Renamed(m_normal1);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal2).negateLocal();
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal2);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal0).negateLocal();
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal1);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal2).negateLocal();
m_upperLimit.set_Renamed(m_normal0).negateLocal();
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal0);
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal0).negateLocal();
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal2);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal1);
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal1);
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal2).negateLocal();
m_upperLimit.set_Renamed(m_normal1);
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal.set_Renamed(m_normal1);
m_lowerLimit.set_Renamed(m_normal1).negateLocal();
m_upperLimit.set_Renamed(m_normal1).negateLocal();
}
else
{
m_normal.set_Renamed(m_normal1).negateLocal();
m_lowerLimit.set_Renamed(m_normal1);
m_upperLimit.set_Renamed(m_normal1);
}
}
// Get polygonB in frameA
m_polygonB.count = polygonB.m_count;
for (int i = 0; i < polygonB.m_count; ++i)
{
Transform.mulToOutUnsafe(m_xf, polygonB.m_vertices[i], m_polygonB.vertices[i]);
Rot.mulToOutUnsafe(m_xf.q, polygonB.m_normals[i], m_polygonB.normals[i]);
}
m_radius = 2.0f * Settings.polygonRadius;
manifold.pointCount = 0;
computeEdgeSeparation(edgeAxis);
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == EPAxis.Type.UNKNOWN)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
computePolygonSeparation(polygonAxis);
if (polygonAxis.type != EPAxis.Type.UNKNOWN && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.type == EPAxis.Type.UNKNOWN)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
// ClipVertex[] ie = new ClipVertex[2];
if (primaryAxis.type == EPAxis.Type.EDGE_A)
{
manifold.type = Manifold.ManifoldType.FACE_A;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vec2.dot(m_normal, m_polygonB.normals[0]);
for (int i = 1; i < m_polygonB.count; ++i)
{
float value_Renamed = Vec2.dot(m_normal, m_polygonB.normals[i]);
if (value_Renamed < bestValue)
{
bestValue = value_Renamed;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v.set_Renamed(m_polygonB.vertices[i1]);
ie[0].id.indexA = 0;
ie[0].id.indexB = (sbyte)i1;
ie[0].id.typeA = (sbyte)ContactID.Type.FACE;
ie[0].id.typeB = (sbyte)ContactID.Type.VERTEX;
ie[1].v.set_Renamed(m_polygonB.vertices[i2]);
ie[1].id.indexA = 0;
ie[1].id.indexB = (sbyte)i2;
ie[1].id.typeA = (sbyte)ContactID.Type.FACE;
ie[1].id.typeB = (sbyte)ContactID.Type.VERTEX;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1.set_Renamed(m_v1);
rf.v2.set_Renamed(m_v2);
rf.normal.set_Renamed(m_normal1);
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1.set_Renamed(m_v2);
rf.v2.set_Renamed(m_v1);
rf.normal.set_Renamed(m_normal1).negateLocal();
}
}
else
{
manifold.type = Manifold.ManifoldType.FACE_B;
ie[0].v.set_Renamed(m_v1);
ie[0].id.indexA = 0;
ie[0].id.indexB = (sbyte)primaryAxis.index;
ie[0].id.typeA = (sbyte)ContactID.Type.VERTEX;
ie[0].id.typeB = (sbyte)ContactID.Type.FACE;
ie[1].v.set_Renamed(m_v2);
ie[1].id.indexA = 0;
ie[1].id.indexB = (sbyte)primaryAxis.index;
ie[1].id.typeA = (sbyte)ContactID.Type.VERTEX;
ie[1].id.typeB = (sbyte)ContactID.Type.FACE;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1.set_Renamed(m_polygonB.vertices[rf.i1]);
rf.v2.set_Renamed(m_polygonB.vertices[rf.i2]);
rf.normal.set_Renamed(m_polygonB.normals[rf.i1]);
}
rf.sideNormal1.set_Renamed(rf.normal.y, -rf.normal.x);
rf.sideNormal2.set_Renamed(rf.sideNormal1).negateLocal();
rf.sideOffset1 = Vec2.dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = Vec2.dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
int np;
// Clip to box side 1
np = clipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < Settings.maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < Settings.maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == EPAxis.Type.EDGE_A)
{
manifold.localNormal.set_Renamed(rf.normal);
manifold.localPoint.set_Renamed(rf.v1);
}
else
{
manifold.localNormal.set_Renamed(polygonB.m_normals[rf.i1]);
manifold.localPoint.set_Renamed(polygonB.m_vertices[rf.i1]);
}
int pointCount = 0;
for (int i = 0; i < Settings.maxManifoldPoints; ++i)
{
float separation;
separation = Vec2.dot(rf.normal, temp.set_Renamed(clipPoints2[i].v).subLocal(rf.v1));
if (separation <= m_radius)
{
ManifoldPoint cp = manifold.points[pointCount];
if (primaryAxis.type == EPAxis.Type.EDGE_A)
{
// cp.localPoint = MulT(m_xf, clipPoints2[i].v);
Transform.mulTransToOutUnsafe(m_xf, clipPoints2[i].v, cp.localPoint);
cp.id.set_Renamed(clipPoints2[i].id);
}
else
{
cp.localPoint.set_Renamed(clipPoints2[i].v);
cp.id.typeA = clipPoints2[i].id.typeB;
cp.id.typeB = clipPoints2[i].id.typeA;
cp.id.indexA = clipPoints2[i].id.indexB;
cp.id.indexB = clipPoints2[i].id.indexA;
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
public virtual void collideEdgeAndCircle(Manifold manifold, EdgeShape edgeA, Transform xfA, CircleShape circleB, Transform xfB)
{
manifold.pointCount = 0;
// Compute circle in frame of edge
// Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p));
Transform.mulToOutUnsafe(xfB, circleB.m_p, temp);
Transform.mulTransToOutUnsafe(xfA, temp, Q);
Vec2 A = edgeA.m_vertex1;
Vec2 B = edgeA.m_vertex2;
e.set_Renamed(B).subLocal(A);
// Barycentric coordinates
float u = Vec2.dot(e, temp.set_Renamed(B).subLocal(Q));
float v = Vec2.dot(e, temp.set_Renamed(Q).subLocal(A));
float radius = edgeA.m_radius + circleB.m_radius;
// ContactFeature cf;
cf.indexB = 0;
cf.typeB = (sbyte)ContactID.Type.VERTEX;
// Region A
if (v <= 0.0f)
{
Vec2 _P = A;
d.set_Renamed(Q).subLocal(_P);
float dd = Vec2.dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.m_hasVertex0)
{
Vec2 A1 = edgeA.m_vertex0;
Vec2 B1 = A;
e1.set_Renamed(B1).subLocal(A1);
float u1 = Vec2.dot(e1, temp.set_Renamed(B1).subLocal(Q));
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = (sbyte)ContactID.Type.VERTEX;
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.CIRCLES;
manifold.localNormal.setZero();
manifold.localPoint.set_Renamed(_P);
// manifold.points[0].id.key = 0;
manifold.points[0].id.set_Renamed(cf);
manifold.points[0].localPoint.set_Renamed(circleB.m_p);
return;
}
// Region B
if (u <= 0.0f)
{
Vec2 _P = B;
d.set_Renamed(Q).subLocal(_P);
float dd = Vec2.dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.m_hasVertex3)
{
Vec2 B2 = edgeA.m_vertex3;
Vec2 A2 = B;
Vec2 e2 = e1;
e2.set_Renamed(B2).subLocal(A2);
float v2 = Vec2.dot(e2, temp.set_Renamed(Q).subLocal(A2));
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = (sbyte)ContactID.Type.VERTEX;
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.CIRCLES;
manifold.localNormal.setZero();
manifold.localPoint.set_Renamed(_P);
// manifold.points[0].id.key = 0;
manifold.points[0].id.set_Renamed(cf);
manifold.points[0].localPoint.set_Renamed(circleB.m_p);
return;
}
// Region AB
float den = Vec2.dot(e, e);
Debug.Assert(den > 0.0f);
// Vec2 P = (1.0f / den) * (u * A + v * B);
P.set_Renamed(A).mulLocal(u).addLocal(temp.set_Renamed(B).mulLocal(v));
P.mulLocal(1.0f / den);
d.set_Renamed(Q).subLocal(P);
float dd2 = Vec2.dot(d, d);
if (dd2 > radius * radius)
{
return;
}
n.x = -e.y;
n.y = e.x;
if (Vec2.dot(n, temp.set_Renamed(Q).subLocal(A)) < 0.0f)
{
n.set_Renamed(-n.x, -n.y);
}
n.normalize();
cf.indexA = 0;
cf.typeA = (sbyte)ContactID.Type.FACE;
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
manifold.localNormal.set_Renamed(n);
manifold.localPoint.set_Renamed(A);
// manifold.points[0].id.key = 0;
manifold.points[0].id.set_Renamed(cf);
manifold.points[0].localPoint.set_Renamed(circleB.m_p);
}
public override Shape Clone()
{
EdgeShape edge = new EdgeShape();
edge.Radius = Radius;
edge.HasVertex0 = HasVertex0;
edge.HasVertex3 = HasVertex3;
edge.Vertex0.Set(Vertex0);
edge.Vertex1.Set(Vertex1);
edge.Vertex2.Set(Vertex2);
edge.Vertex3.Set(Vertex3);
return edge;
}
public override Shape Clone()
{
EdgeShape edge = new EdgeShape();
edge.m_radius = this.m_radius;
edge.m_hasVertex0 = this.m_hasVertex0;
edge.m_hasVertex3 = this.m_hasVertex3;
edge.m_vertex0.set_Renamed(this.m_vertex0);
edge.m_vertex1.set_Renamed(this.m_vertex1);
edge.m_vertex2.set_Renamed(this.m_vertex2);
edge.m_vertex3.set_Renamed(this.m_vertex3);
return edge;
}
/// <summary>
/// Get a child edge.
/// </summary>
public virtual void getChildEdge(EdgeShape edge, int index)
{
Debug.Assert(0 <= index && index < m_count - 1);
edge.m_radius = m_radius;
edge.m_vertex1.set_Renamed(m_vertices[index + 0]);
edge.m_vertex2.set_Renamed(m_vertices[index + 1]);
if (index > 0)
{
edge.m_vertex0.set_Renamed(m_vertices[index - 1]);
edge.m_hasVertex0 = true;
}
else
{
edge.m_vertex0.set_Renamed(m_prevVertex);
edge.m_hasVertex0 = m_hasPrevVertex;
}
if (index < m_count - 2)
{
edge.m_vertex3.set_Renamed(m_vertices[index + 2]);
edge.m_hasVertex3 = true;
}
else
{
edge.m_vertex3.set_Renamed(m_nextVertex);
edge.m_hasVertex3 = m_hasNextVertex;
}
}
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
public void CollideEdgeAndCircle(Manifold manifold, EdgeShape edgeA, Transform xfA, CircleShape circleB, Transform xfB)
{
manifold.PointCount = 0;
// Compute circle in frame of edge
// Vec2 Q = MulT(xfA, Mul(xfB, circleB.m_p));
Transform.MulToOutUnsafe(xfB, circleB.P, temp);
Transform.MulTransToOutUnsafe(xfA, temp, q);
Vec2 A = edgeA.Vertex1;
Vec2 B = edgeA.Vertex2;
e.Set(B).SubLocal(A);
// Barycentric coordinates
float u = Vec2.Dot(e, temp.Set(B).SubLocal(q));
float v = Vec2.Dot(e, temp.Set(q).SubLocal(A));
float radius = edgeA.Radius + circleB.Radius;
// ContactFeature cf;
cf.IndexB = 0;
cf.TypeB = (sbyte)ContactID.Type.Vertex;
// Region A
if (v <= 0.0f)
{
Vec2 P = A;
D.Set(q).SubLocal(P);
float dd = Vec2.Dot(D, D);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
Vec2 A1 = edgeA.Vertex0;
Vec2 B1 = A;
e1.Set(B1).SubLocal(A1);
float u1 = Vec2.Dot(e1, temp.Set(B1).SubLocal(q));
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.IndexA = 0;
cf.TypeA = (sbyte)ContactID.Type.Vertex;
manifold.PointCount = 1;
manifold.Type = Manifold.ManifoldType.Circles;
manifold.LocalNormal.SetZero();
manifold.LocalPoint.Set(P);
// manifold.points[0].id.key = 0;
manifold.Points[0].Id.Set(cf);
manifold.Points[0].LocalPoint.Set(circleB.P);
return;
}
// Region B
if (u <= 0.0f)
{
Vec2 P = B;
D.Set(q).SubLocal(P);
float dd = Vec2.Dot(D, D);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
Vec2 B2 = edgeA.Vertex3;
Vec2 A2 = B;
Vec2 e2 = e1;
e2.Set(B2).SubLocal(A2);
float v2 = Vec2.Dot(e2, temp.Set(q).SubLocal(A2));
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.IndexA = 1;
cf.TypeA = (sbyte)ContactID.Type.Vertex;
manifold.PointCount = 1;
manifold.Type = Manifold.ManifoldType.Circles;
manifold.LocalNormal.SetZero();
manifold.LocalPoint.Set(P);
// manifold.points[0].id.key = 0;
manifold.Points[0].Id.Set(cf);
manifold.Points[0].LocalPoint.Set(circleB.P);
return;
}
// Region AB
float den = Vec2.Dot(e, e);
Debug.Assert(den > 0.0f);
// Vec2 P = (1.0f / den) * (u * A + v * B);
p.Set(A).MulLocal(u).AddLocal(temp.Set(B).MulLocal(v));
p.MulLocal(1.0f / den);
D.Set(q).SubLocal(p);
float dd2 = Vec2.Dot(D, D);
if (dd2 > radius * radius)
{
return;
}
n.X = -e.Y;
n.Y = e.X;
if (Vec2.Dot(n, temp.Set(q).SubLocal(A)) < 0.0f)
{
n.Set(-n.X, -n.Y);
}
n.Normalize();
cf.IndexA = 0;
cf.TypeA = (sbyte)ContactID.Type.Face;
manifold.PointCount = 1;
manifold.Type = Manifold.ManifoldType.FaceA;
manifold.LocalNormal.Set(n);
manifold.LocalPoint.Set(A);
// manifold.points[0].id.key = 0;
manifold.Points[0].Id.Set(cf);
manifold.Points[0].LocalPoint.Set(circleB.P);
}
public void Collide(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB)
{
Transform.MulTransToOutUnsafe(xfA, xfB, xf);
Transform.MulToOutUnsafe(xf, polygonB.Centroid, centroidB);
v0 = edgeA.Vertex0;
v1 = edgeA.Vertex1;
v2 = edgeA.Vertex2;
v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
edge1.Set(v2).SubLocal(v1);
edge1.Normalize();
normal1.Set(edge1.Y, -edge1.X);
float offset1 = Vec2.Dot(normal1, temp.Set(centroidB).SubLocal(v1));
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
edge0.Set(v1).SubLocal(v0);
edge0.Normalize();
normal0.Set(edge0.Y, -edge0.X);
convex1 = Vec2.Cross(edge0, edge1) >= 0.0f;
offset0 = Vec2.Dot(normal0, temp.Set(centroidB).SubLocal(v0));
}
// Is there a following edge?
if (hasVertex3)
{
edge2.Set(v3).SubLocal(v2);
edge2.Normalize();
normal2.Set(edge2.Y, -edge2.X);
convex2 = Vec2.Cross(edge1, edge2) > 0.0f;
offset2 = Vec2.Dot(normal2, temp.Set(centroidB).SubLocal(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.Set(normal1);
lowerLimit.Set(normal0);
upperLimit.Set(normal2);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal1).NegateLocal();
}
}
else if (convex1)
{
front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal0);
upperLimit.Set(normal1);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal2).NegateLocal();
upperLimit.Set(normal1).NegateLocal();
}
}
else if (convex2)
{
front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1);
upperLimit.Set(normal2);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal0).NegateLocal();
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1);
upperLimit.Set(normal1);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal2).NegateLocal();
upperLimit.Set(normal0).NegateLocal();
}
}
}
else if (hasVertex0)
{
if (convex1)
{
front = offset0 >= 0.0f || offset1 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal0);
upperLimit.Set(normal1).NegateLocal();
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1);
upperLimit.Set(normal1).NegateLocal();
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1);
upperLimit.Set(normal1).NegateLocal();
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1);
upperLimit.Set(normal0).NegateLocal();
}
}
}
else if (hasVertex3)
{
if (convex2)
{
front = offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal2);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal1);
}
}
else
{
front = offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal1);
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal2).NegateLocal();
upperLimit.Set(normal1);
}
}
}
else
{
front = offset1 >= 0.0f;
if (front)
{
normal.Set(normal1);
lowerLimit.Set(normal1).NegateLocal();
upperLimit.Set(normal1).NegateLocal();
}
else
{
normal.Set(normal1).NegateLocal();
lowerLimit.Set(normal1);
upperLimit.Set(normal1);
}
}
// Get polygonB in frameA
this.polygonB.Count = polygonB.VertexCount;
for (int i = 0; i < polygonB.VertexCount; ++i)
{
Transform.MulToOutUnsafe(xf, polygonB.Vertices[i], this.polygonB.Vertices[i]);
Rot.MulToOutUnsafe(xf.Q, polygonB.Normals[i], this.polygonB.Normals[i]);
}
radius = 2.0f * Settings.POLYGON_RADIUS;
manifold.PointCount = 0;
ComputeEdgeSeparation(edgeAxis);
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == EPAxis.EPAxisType.Unknown)
{
return;
}
if (edgeAxis.Separation > radius)
{
return;
}
ComputePolygonSeparation(polygonAxis);
if (polygonAxis.Type != EPAxis.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 == EPAxis.EPAxisType.Unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
// ClipVertex[] ie = new ClipVertex[2];
if (primaryAxis.Type == EPAxis.EPAxisType.EdgeA)
{
manifold.Type = Manifold.ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vec2.Dot(normal, this.polygonB.Normals[0]);
for (int i = 1; i < this.polygonB.Count; ++i)
{
float value = Vec2.Dot(normal, this.polygonB.Normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < this.polygonB.Count ? i1 + 1 : 0;
ie[0].V.Set(this.polygonB.Vertices[i1]);
ie[0].Id.IndexA = 0;
ie[0].Id.IndexB = (sbyte)i1;
ie[0].Id.TypeA = (sbyte)ContactID.Type.Face;
ie[0].Id.TypeB = (sbyte)ContactID.Type.Vertex;
ie[1].V.Set(this.polygonB.Vertices[i2]);
ie[1].Id.IndexA = 0;
ie[1].Id.IndexB = (sbyte)i2;
ie[1].Id.TypeA = (sbyte)ContactID.Type.Face;
ie[1].Id.TypeB = (sbyte)ContactID.Type.Vertex;
if (front)
{
rf.I1 = 0;
rf.I2 = 1;
rf.V1.Set(v1);
rf.V2.Set(v2);
rf.Normal.Set(normal1);
}
else
{
rf.I1 = 1;
rf.I2 = 0;
rf.V1.Set(v2);
rf.V2.Set(v1);
rf.Normal.Set(normal1).NegateLocal();
}
}
else
{
manifold.Type = Manifold.ManifoldType.FaceB;
ie[0].V.Set(v1);
ie[0].Id.IndexA = 0;
ie[0].Id.IndexB = (sbyte)primaryAxis.Index;
ie[0].Id.TypeA = (sbyte)ContactID.Type.Vertex;
ie[0].Id.TypeB = (sbyte)ContactID.Type.Face;
ie[1].V.Set(v2);
ie[1].Id.IndexA = 0;
ie[1].Id.IndexB = (sbyte)primaryAxis.Index;
ie[1].Id.TypeA = (sbyte)ContactID.Type.Vertex;
ie[1].Id.TypeB = (sbyte)ContactID.Type.Face;
rf.I1 = primaryAxis.Index;
rf.I2 = rf.I1 + 1 < this.polygonB.Count ? rf.I1 + 1 : 0;
rf.V1.Set(this.polygonB.Vertices[rf.I1]);
rf.V2.Set(this.polygonB.Vertices[rf.I2]);
rf.Normal.Set(this.polygonB.Normals[rf.I1]);
}
rf.SideNormal1.Set(rf.Normal.Y, -rf.Normal.X);
rf.SideNormal2.Set(rf.SideNormal1).NegateLocal();
rf.SideOffset1 = Vec2.Dot(rf.SideNormal1, rf.V1);
rf.SideOffset2 = Vec2.Dot(rf.SideNormal2, rf.V2);
// Clip incident edge against extruded edge1 side edges.
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, ie, rf.SideNormal1, rf.SideOffset1, rf.I1);
if (np < Settings.MAX_MANIFOLD_POINTS)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.I2);
if (np < Settings.MAX_MANIFOLD_POINTS)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxis.EPAxisType.EdgeA)
{
manifold.LocalNormal.Set(rf.Normal);
manifold.LocalPoint.Set(rf.V1);
}
else
{
manifold.LocalNormal.Set(polygonB.Normals[rf.I1]);
manifold.LocalPoint.Set(polygonB.Vertices[rf.I1]);
}
int pointCount = 0;
for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; ++i)
{
float separation = Vec2.Dot(rf.Normal, temp.Set(clipPoints2[i].V).SubLocal(rf.V1));
if (separation <= radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxis.EPAxisType.EdgeA)
{
// cp.localPoint = MulT(m_xf, clipPoints2[i].v);
Transform.MulTransToOutUnsafe(xf, clipPoints2[i].V, cp.LocalPoint);
cp.Id.Set(clipPoints2[i].Id);
}
else
{
cp.LocalPoint.Set(clipPoints2[i].V);
cp.Id.TypeA = clipPoints2[i].Id.TypeB;
cp.Id.TypeB = clipPoints2[i].Id.TypeA;
cp.Id.IndexA = clipPoints2[i].Id.IndexB;
cp.Id.IndexB = clipPoints2[i].Id.IndexA;
}
++pointCount;
}
}
manifold.PointCount = pointCount;
}
