protected internal Contact(IWorldPool argPool)
{
FixtureA = null;
FixtureB = null;
NodeA = new ContactEdge();
NodeB = new ContactEdge();
Manifold = new Manifold();
Pool = argPool;
}
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;
}
protected internal Contact(IWorldPool argPool)
{
m_fixtureA = null;
m_fixtureB = null;
m_nodeA = new ContactEdge();
m_nodeB = new ContactEdge();
m_manifold = new Manifold();
pool = argPool;
}
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;
}
public override void evaluate(Manifold manifold, Transform xfA, Transform xfB)
{
pool.getCollision().collideCircles(manifold, (CircleShape)m_fixtureA.Shape, xfA, (CircleShape)m_fixtureB.Shape, xfB);
}
/// <summary>
/// copies this manifold from the given one
/// </summary>
/// <param name="cp">manifold to copy from
/// </param>
public virtual void set_Renamed(Manifold cp)
{
for (int i = 0; i < cp.pointCount; i++)
{
points[i].set_Renamed(cp.points[i]);
}
type = cp.type;
localNormal.set_Renamed(cp.localNormal);
localPoint.set_Renamed(cp.localPoint);
pointCount = cp.pointCount;
}
/// <summary>
/// Compute the collision manifold between a polygon and a circle.
/// </summary>
/// <param name="manifold"></param>
/// <param name="polygon"></param>
/// <param name="xfA"></param>
/// <param name="circle"></param>
/// <param name="xfB"></param>
public void collidePolygonAndCircle(Manifold manifold, PolygonShape polygon, Transform xfA, CircleShape circle, Transform xfB)
{
manifold.pointCount = 0;
//Vec2 v = circle.m_p;
// Compute circle position in the frame of the polygon.
// before inline:
Transform.mulToOut(xfB, circle.m_p, c);
Transform.mulTransToOut(xfA, c, cLocal);
float cLocalx = cLocal.x;
float cLocaly = cLocal.y;
// after inline:
// final float cy = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y;
// final float cx = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y;
// final float v1x = cx - xfA.p.x;
// final float v1y = cy - xfA.p.y;
// final Vec2 b = xfA.q.ex;
// final Vec2 b1 = xfA.q.ey;
// final float cLocaly = v1x * b1.x + v1y * b1.y;
// final float cLocalx = v1x * b.x + v1y * b.y;
// end inline
// Find the min separating edge.
int normalIndex = 0;
//UPGRADE_TODO: The equivalent in .NET for field 'java.lang.Float.MIN_VALUE' may return a different value. "ms-help://MS.VSCC.v80/dv_commoner/local/redirect.htm?index='!DefaultContextWindowIndex'&keyword='jlca1043'"
float separation = Single.Epsilon;
float radius = polygon.m_radius + circle.m_radius;
int vertexCount = polygon.m_count;
Vec2[] vertices = polygon.m_vertices;
Vec2[] normals = polygon.m_normals;
for (int i = 0; i < vertexCount; i++)
{
// before inline
// temp.set(cLocal).subLocal(vertices[i]);
// float s = Vec2.dot(normals[i], temp);
// after inline
Vec2 vertex = vertices[i];
float tempx = cLocalx - vertex.x;
float tempy = cLocaly - vertex.y;
Vec2 normal = normals[i];
float s = normal.x * tempx + normal.y * tempy;
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 < Settings.EPSILON)
{
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
// before inline:
// manifold.localNormal.set(normals[normalIndex]);
// manifold.localPoint.set(v1).addLocal(v2).mulLocal(.5f);
// manifold.points[0].localPoint.set(circle.m_p);
// after inline:
Vec2 normal = normals[normalIndex];
manifold.localNormal.x = normal.x;
manifold.localNormal.y = normal.y;
manifold.localPoint.x = (v1.x + v2.x) * .5f;
manifold.localPoint.y = (v1.y + v2.y) * .5f;
ManifoldPoint mpoint = manifold.points[0];
mpoint.localPoint.x = circle.m_p.x;
mpoint.localPoint.y = circle.m_p.y;
mpoint.id.zero();
// end inline
return;
}
// Compute barycentric coordinates
// before inline:
// temp.set(cLocal).subLocal(v1);
// temp2.set(v2).subLocal(v1);
// float u1 = Vec2.dot(temp, temp2);
// temp.set(cLocal).subLocal(v2);
// temp2.set(v1).subLocal(v2);
// float u2 = Vec2.dot(temp, temp2);
// after inline:
float tempX = cLocalx - v1.x;
float tempY = cLocaly - v1.y;
float temp2X = v2.x - v1.x;
float temp2Y = v2.y - v1.y;
float u1 = tempX * temp2X + tempY * temp2Y;
float temp3X = cLocalx - v2.x;
float temp3Y = cLocaly - v2.y;
float temp4X = v1.x - v2.x;
float temp4Y = v1.y - v2.y;
float u2 = temp3X * temp4X + temp3Y * temp4Y;
// end inline
if (u1 <= 0f)
{
// inlined
float dx = cLocalx - v1.x;
float dy = cLocaly - v1.y;
if (dx * dx + dy * dy > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
// before inline:
// manifold.localNormal.set(cLocal).subLocal(v1);
// after inline:
manifold.localNormal.x = cLocalx - v1.x;
manifold.localNormal.y = cLocaly - v1.y;
// end inline
manifold.localNormal.normalize();
manifold.localPoint.set_Renamed(v1);
manifold.points[0].localPoint.set_Renamed(circle.m_p);
manifold.points[0].id.zero();
}
else if (u2 <= 0.0f)
{
// inlined
float dx = cLocalx - v2.x;
float dy = cLocaly - v2.y;
if (dx * dx + dy * dy > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
// before inline:
// manifold.localNormal.set(cLocal).subLocal(v2);
// after inline:
manifold.localNormal.x = cLocalx - v2.x;
manifold.localNormal.y = cLocaly - v2.y;
// end inline
manifold.localNormal.normalize();
manifold.localPoint.set_Renamed(v2);
manifold.points[0].localPoint.set_Renamed(circle.m_p);
manifold.points[0].id.zero();
}
else
{
// Vec2 faceCenter = 0.5f * (v1 + v2);
// (temp is faceCenter)
// before inline:
// temp.set(v1).addLocal(v2).mulLocal(.5f);
//
// temp2.set(cLocal).subLocal(temp);
// separation = Vec2.dot(temp2, normals[vertIndex1]);
// if (separation > radius) {
// return;
// }
// after inline:
float fcx = (v1.x + v2.x) * .5f;
float fcy = (v1.y + v2.y) * .5f;
float tx = cLocalx - fcx;
float ty = cLocaly - fcy;
Vec2 normal = normals[vertIndex1];
separation = tx * normal.x + ty * normal.y;
if (separation > radius)
{
return;
}
// end inline
manifold.pointCount = 1;
manifold.type = Manifold.ManifoldType.FACE_A;
manifold.localNormal.set_Renamed(normals[vertIndex1]);
manifold.localPoint.x = fcx; // (faceCenter)
manifold.localPoint.y = fcy;
manifold.points[0].localPoint.set_Renamed(circle.m_p);
manifold.points[0].id.zero();
}
}
// 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 void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB)
{
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case Manifold.ManifoldType.Circles:
{
// final Vec2 pointA = pool3;
// final Vec2 pointB = pool4;
//
// normal.set(1, 0);
// Transform.mulToOut(xfA, manifold.localPoint, pointA);
// Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB);
//
// if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) {
// normal.set(pointB).subLocal(pointA);
// normal.normalize();
// }
//
// cA.set(normal).mulLocal(radiusA).addLocal(pointA);
// cB.set(normal).mulLocal(radiusB).subLocal(pointB).negateLocal();
// points[0].set(cA).addLocal(cB).mulLocal(0.5f);
Vec2 pointA = pool3;
Vec2 pointB = pool4;
Normal.X = 1;
Normal.Y = 0;
// pointA.x = xfA.p.x + xfA.q.ex.x * manifold.localPoint.x + xfA.q.ey.x *
// manifold.localPoint.y;
// pointA.y = xfA.p.y + xfA.q.ex.y * manifold.localPoint.x + xfA.q.ey.y *
// manifold.localPoint.y;
// pointB.x = xfB.p.x + xfB.q.ex.x * manifold.points[0].localPoint.x + xfB.q.ey.x *
// manifold.points[0].localPoint.y;
// pointB.y = xfB.p.y + xfB.q.ex.y * manifold.points[0].localPoint.x + xfB.q.ey.y *
// manifold.points[0].localPoint.y;
Transform.MulToOut(xfA, manifold.LocalPoint, pointA);
Transform.MulToOut(xfB, manifold.Points[0].LocalPoint, pointB);
if (MathUtils.DistanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON)
{
Normal.X = pointB.X - pointA.X;
Normal.Y = pointB.Y - pointA.Y;
Normal.Normalize();
}
float cAx = Normal.X * radiusA + pointA.X;
float cAy = Normal.Y * radiusA + pointA.Y;
float cBx = (-Normal.X) * radiusB + pointB.X;
float cBy = (-Normal.Y) * radiusB + pointB.Y;
Points[0].X = (cAx + cBx) * .5f;
Points[0].Y = (cAy + cBy) * .5f;
}
break;
case Manifold.ManifoldType.FaceA:
{
Vec2 planePoint = pool3;
Rot.MulToOutUnsafe(xfA.Q, manifold.LocalNormal, Normal);
Transform.MulToOut(xfA, manifold.LocalPoint, planePoint);
Vec2 clipPoint = pool4;
for (int i = 0; i < manifold.PointCount; i++)
{
// b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
// b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
// normal)) * normal;
// b2Vec2 cB = clipPoint - radiusB * normal;
// points[i] = 0.5f * (cA + cB);
Transform.MulToOut(xfB, manifold.Points[i].LocalPoint, clipPoint);
// use cA as temporary for now
// cA.set(clipPoint).subLocal(planePoint);
// float scalar = radiusA - Vec2.dot(cA, normal);
// cA.set(normal).mulLocal(scalar).addLocal(clipPoint);
// cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal();
// points[i].set(cA).addLocal(cB).mulLocal(0.5f);
float scalar = radiusA - ((clipPoint.X - planePoint.X) * Normal.X + (clipPoint.Y - planePoint.Y) * Normal.Y);
float cAx = Normal.X * scalar + clipPoint.X;
float cAy = Normal.Y * scalar + clipPoint.Y;
float cBx = (-Normal.X) * radiusB + clipPoint.X;
float cBy = (-Normal.Y) * radiusB + clipPoint.Y;
Points[i].X = (cAx + cBx) * .5f;
Points[i].Y = (cAy + cBy) * .5f;
}
}
break;
case Manifold.ManifoldType.FaceB:
Vec2 planePoint2 = pool3;
Rot.MulToOutUnsafe(xfB.Q, manifold.LocalNormal, Normal);
Transform.MulToOut(xfB, manifold.LocalPoint, planePoint2);
// final Mat22 R = xfB.q;
// normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y;
// normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y;
// final Vec2 v = manifold.localPoint;
// planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y;
// planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y;
Vec2 clipPoint2 = pool4;
for (int i = 0; i < manifold.PointCount; i++)
{
// b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
// b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint,
// normal)) * normal;
// b2Vec2 cA = clipPoint - radiusA * normal;
// points[i] = 0.5f * (cA + cB);
Transform.MulToOut(xfA, manifold.Points[i].LocalPoint, clipPoint2);
// cB.set(clipPoint).subLocal(planePoint);
// float scalar = radiusB - Vec2.dot(cB, normal);
// cB.set(normal).mulLocal(scalar).addLocal(clipPoint);
// cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal();
// points[i].set(cA).addLocal(cB).mulLocal(0.5f);
// points[i] = 0.5f * (cA + cB);
//
// clipPoint.x = xfA.p.x + xfA.q.ex.x * manifold.points[i].localPoint.x + xfA.q.ey.x *
// manifold.points[i].localPoint.y;
// clipPoint.y = xfA.p.y + xfA.q.ex.y * manifold.points[i].localPoint.x + xfA.q.ey.y *
// manifold.points[i].localPoint.y;
float scalar = radiusB - ((clipPoint2.X - planePoint2.X) * Normal.X + (clipPoint2.Y - planePoint2.Y) * Normal.Y);
float cBx = Normal.X * scalar + clipPoint2.X;
float cBy = Normal.Y * scalar + clipPoint2.Y;
float cAx = (-Normal.X) * radiusA + clipPoint2.X;
float cAy = (-Normal.Y) * radiusA + clipPoint2.Y;
Points[i].X = (cAx + cBx) * .5f;
Points[i].Y = (cAy + cBy) * .5f;
}
// Ensure normal points from A to B.
Normal.X = -Normal.X;
Normal.Y = -Normal.Y;
break;
}
}
/// <summary>
/// copies this manifold from the given one
/// </summary>
/// <param name="cp">manifold to copy from</param>
public void Set(Manifold cp)
{
for (int i = 0; i < cp.PointCount; i++)
{
Points[i].Set(cp.Points[i]);
}
Type = cp.Type;
LocalNormal.Set(cp.LocalNormal);
LocalPoint.Set(cp.LocalPoint);
PointCount = cp.PointCount;
}
/// <summary>
/// Creates this manifold as a copy of the other
/// </summary>
/// <param name="other"></param>
public Manifold(Manifold other)
{
Points = new ManifoldPoint[Settings.MAX_MANIFOLD_POINTS];
LocalNormal = other.LocalNormal.Clone();
LocalPoint = other.LocalPoint.Clone();
PointCount = other.PointCount;
Type = other.Type;
// djm: this is correct now
for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; i++)
{
Points[i] = new ManifoldPoint(other.Points[i]);
}
}
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:
{
// final Vec2 pointA = pool3;
// final Vec2 pointB = pool4;
//
// normal.set(1, 0);
// Transform.mulToOut(xfA, manifold.localPoint, pointA);
// Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB);
//
// if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) {
// normal.set(pointB).subLocal(pointA);
// normal.normalize();
// }
//
// cA.set(normal).mulLocal(radiusA).addLocal(pointA);
// cB.set(normal).mulLocal(radiusB).subLocal(pointB).negateLocal();
// points[0].set(cA).addLocal(cB).mulLocal(0.5f);
Vec2 pointA = pool3;
Vec2 pointB = pool4;
normal.x = 1;
normal.y = 0;
// pointA.x = xfA.p.x + xfA.q.ex.x * manifold.localPoint.x + xfA.q.ey.x *
// manifold.localPoint.y;
// pointA.y = xfA.p.y + xfA.q.ex.y * manifold.localPoint.x + xfA.q.ey.y *
// manifold.localPoint.y;
// pointB.x = xfB.p.x + xfB.q.ex.x * manifold.points[0].localPoint.x + xfB.q.ey.x *
// manifold.points[0].localPoint.y;
// pointB.y = xfB.p.y + xfB.q.ex.y * manifold.points[0].localPoint.x + xfB.q.ey.y *
// manifold.points[0].localPoint.y;
Transform.mulToOut(xfA, manifold.localPoint, pointA);
Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB);
if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON)
{
normal.x = pointB.x - pointA.x;
normal.y = pointB.y - pointA.y;
normal.normalize();
}
float cAx = normal.x * radiusA + pointA.x;
float cAy = normal.y * radiusA + pointA.y;
float cBx = (-normal.x) * radiusB + pointB.x;
float cBy = (-normal.y) * radiusB + pointB.y;
points[0].x = (cAx + cBx) * .5f;
points[0].y = (cAy + cBy) * .5f;
}
break;
case Manifold.ManifoldType.FACE_A:
{
Vec2 planePoint = pool3;
Rot.mulToOutUnsafe(xfA.q, manifold.localNormal, normal);
Transform.mulToOut(xfA, manifold.localPoint, planePoint);
Vec2 clipPoint = pool4;
for (int i = 0; i < manifold.pointCount; i++)
{
// b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
// b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
// normal)) * normal;
// b2Vec2 cB = clipPoint - radiusB * normal;
// points[i] = 0.5f * (cA + cB);
Transform.mulToOut(xfB, manifold.points[i].localPoint, clipPoint);
// use cA as temporary for now
// cA.set(clipPoint).subLocal(planePoint);
// float scalar = radiusA - Vec2.dot(cA, normal);
// cA.set(normal).mulLocal(scalar).addLocal(clipPoint);
// cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal();
// points[i].set(cA).addLocal(cB).mulLocal(0.5f);
float scalar = radiusA - ((clipPoint.x - planePoint.x) * normal.x + (clipPoint.y - planePoint.y) * normal.y);
float cAx = normal.x * scalar + clipPoint.x;
float cAy = normal.y * scalar + clipPoint.y;
float cBx = (-normal.x) * radiusB + clipPoint.x;
float cBy = (-normal.y) * radiusB + clipPoint.y;
points[i].x = (cAx + cBx) * .5f;
points[i].y = (cAy + cBy) * .5f;
}
}
break;
case Manifold.ManifoldType.FACE_B:
Vec2 planePoint2 = pool3;
Rot.mulToOutUnsafe(xfB.q, manifold.localNormal, normal);
Transform.mulToOut(xfB, manifold.localPoint, planePoint2);
// final Mat22 R = xfB.q;
// normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y;
// normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y;
// final Vec2 v = manifold.localPoint;
// planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y;
// planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y;
Vec2 clipPoint2 = pool4;
for (int i = 0; i < manifold.pointCount; i++)
{
// b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
// b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint,
// normal)) * normal;
// b2Vec2 cA = clipPoint - radiusA * normal;
// points[i] = 0.5f * (cA + cB);
Transform.mulToOut(xfA, manifold.points[i].localPoint, clipPoint2);
// cB.set(clipPoint).subLocal(planePoint);
// float scalar = radiusB - Vec2.dot(cB, normal);
// cB.set(normal).mulLocal(scalar).addLocal(clipPoint);
// cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal();
// points[i].set(cA).addLocal(cB).mulLocal(0.5f);
// points[i] = 0.5f * (cA + cB);
//
// clipPoint.x = xfA.p.x + xfA.q.ex.x * manifold.points[i].localPoint.x + xfA.q.ey.x *
// manifold.points[i].localPoint.y;
// clipPoint.y = xfA.p.y + xfA.q.ex.y * manifold.points[i].localPoint.x + xfA.q.ey.y *
// manifold.points[i].localPoint.y;
float scalar = radiusB - ((clipPoint2.x - planePoint2.x) * normal.x + (clipPoint2.y - planePoint2.y) * normal.y);
float cBx = normal.x * scalar + clipPoint2.x;
float cBy = normal.y * scalar + clipPoint2.y;
float cAx = (-normal.x) * radiusA + clipPoint2.x;
float cAy = (-normal.y) * radiusA + clipPoint2.y;
points[i].x = (cAx + cBx) * .5f;
points[i].y = (cAy + cBy) * .5f;
}
// Ensure normal points from A to B.
normal.x = -normal.x;
normal.y = -normal.y;
break;
}
}
// 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);
}
/// <summary>
/// Compute the collision manifold between two circles.
/// </summary>
/// <param name="manifold"></param>
/// <param name="circle1"></param>
/// <param name="xfA"></param>
/// <param name="circle2"></param>
/// <param name="xfB"></param>
public void CollideCircles(Manifold manifold, CircleShape circle1, Transform xfA, CircleShape circle2, Transform xfB)
{
manifold.PointCount = 0;
// before inline:
Transform.MulToOut(xfA, circle1.P, P_A);
Transform.MulToOut(xfB, circle2.P, P_B);
D.Set(P_B).SubLocal(P_A);
float distSqr = D.X * D.X + D.Y * D.Y;
// after inline:
// final Vec2 v = circle1.m_p;
// final float pAy = xfA.p.y + xfA.q.ex.y * v.x + xfA.q.ey.y * v.y;
// final float pAx = xfA.p.x + xfA.q.ex.x * v.x + xfA.q.ey.x * v.y;
//
// final Vec2 v1 = circle2.m_p;
// final float pBy = xfB.p.y + xfB.q.ex.y * v1.x + xfB.q.ey.y * v1.y;
// final float pBx = xfB.p.x + xfB.q.ex.x * v1.x + xfB.q.ey.x * v1.y;
//
// final float dx = pBx - pAx;
// final float dy = pBy - pAy;
//
// final float distSqr = dx * dx + dy * dy;
// end inline
float radius = circle1.Radius + circle2.Radius;
if (distSqr > radius * radius)
{
return;
}
manifold.Type = Manifold.ManifoldType.Circles;
manifold.LocalPoint.Set(circle1.P);
manifold.LocalNormal.SetZero();
manifold.PointCount = 1;
manifold.Points[0].LocalPoint.Set(circle2.P);
manifold.Points[0].Id.Zero();
}
/// <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>
/// <param name="state1"></param>
/// <param name="state2"></param>
/// <param name="manifold1"></param>
/// <param name="manifold2"></param>
public static void GetPointStates(PointState[] state1, PointState[] state2, Manifold manifold1, Manifold manifold2)
{
for (int i = 0; i < Settings.MAX_MANIFOLD_POINTS; 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.IsEqual(id))
{
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.IsEqual(id))
{
state2[i] = PointState.PersistState;
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>
/// <param name="state1"></param>
/// <param name="state2"></param>
/// <param name="manifold1"></param>
/// <param name="manifold2"></param>
public static void getPointStates(PointState[] state1, PointState[] state2, Manifold manifold1, Manifold manifold2)
{
for (int i = 0; i < Settings.maxManifoldPoints; i++)
{
state1[i] = PointState.NULL_STATE;
state2[i] = PointState.NULL_STATE;
}
// Detect persists and removes.
for (int i = 0; i < manifold1.pointCount; i++)
{
ContactID id = manifold1.points[i].id;
state1[i] = PointState.REMOVE_STATE;
for (int j = 0; j < manifold2.pointCount; j++)
{
if (manifold2.points[j].id.isEqual(id))
{
state1[i] = PointState.PERSIST_STATE;
break;
}
}
}
// Detect persists and adds
for (int i = 0; i < manifold2.pointCount; i++)
{
ContactID id = manifold2.points[i].id;
state2[i] = PointState.ADD_STATE;
for (int j = 0; j < manifold1.pointCount; j++)
{
if (manifold1.points[j].id.isEqual(id))
{
state2[i] = PointState.PERSIST_STATE;
break;
}
}
}
}
/// <summary>
/// Compute the collision manifold between two circles.
/// </summary>
/// <param name="manifold"></param>
/// <param name="circle1"></param>
/// <param name="xfA"></param>
/// <param name="circle2"></param>
/// <param name="xfB"></param>
public void collideCircles(Manifold manifold, CircleShape circle1, Transform xfA, CircleShape circle2, Transform xfB)
{
manifold.pointCount = 0;
// before inline:
Transform.mulToOut(xfA, circle1.m_p, pA);
Transform.mulToOut(xfB, circle2.m_p, pB);
d.set_Renamed(pB).subLocal(pA);
float distSqr = d.x * d.x + d.y * d.y;
// after inline:
// final Vec2 v = circle1.m_p;
// final float pAy = xfA.p.y + xfA.q.ex.y * v.x + xfA.q.ey.y * v.y;
// final float pAx = xfA.p.x + xfA.q.ex.x * v.x + xfA.q.ey.x * v.y;
//
// final Vec2 v1 = circle2.m_p;
// final float pBy = xfB.p.y + xfB.q.ex.y * v1.x + xfB.q.ey.y * v1.y;
// final float pBx = xfB.p.x + xfB.q.ex.x * v1.x + xfB.q.ey.x * v1.y;
//
// final float dx = pBx - pAx;
// final float dy = pBy - pAy;
//
// final float distSqr = dx * dx + dy * dy;
// end inline
float radius = circle1.m_radius + circle2.m_radius;
if (distSqr > radius * radius)
{
return;
}
manifold.type = Manifold.ManifoldType.CIRCLES;
manifold.localPoint.set_Renamed(circle1.m_p);
manifold.localNormal.setZero();
manifold.pointCount = 1;
manifold.points[0].localPoint.set_Renamed(circle2.m_p);
manifold.points[0].id.zero();
}
public override void evaluate(Manifold manifold, Transform xfA, Transform xfB)
{
ChainShape chain = (ChainShape)m_fixtureA.Shape;
chain.getChildEdge(edge, m_indexA);
pool.getCollision().collideEdgeAndPolygon(manifold, edge, xfA, (PolygonShape)m_fixtureB.Shape, xfB);
}
public virtual void collideEdgeAndPolygon(Manifold manifold, EdgeShape edgeA, Transform xfA, PolygonShape polygonB, Transform xfB)
{
collider.collide(manifold, edgeA, xfA, polygonB, xfB);
}
public override void Evaluate(Manifold manifold, Transform xfA, Transform xfB)
{
ChainShape chain = (ChainShape)FixtureA.Shape;
chain.GetChildEdge(edge, ChildIndexA);
Pool.GetCollision().CollideEdgeAndPolygon(manifold, edge, xfA, (PolygonShape)FixtureB.Shape, xfB);
}
/// <summary>
/// Compute the collision manifold between two polygons.
/// </summary>
/// <param name="manifold"></param>
/// <param name="polygon1"></param>
/// <param name="xf1"></param>
/// <param name="polygon2"></param>
/// <param name="xf2"></param>
public void collidePolygons(Manifold manifold, PolygonShape polyA, Transform xfA, PolygonShape polyB, 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
// The normal points from 1 to 2
manifold.pointCount = 0;
float totalRadius = polyA.m_radius + polyB.m_radius;
findMaxSeparation(results1, polyA, xfA, polyB, xfB);
if (results1.separation > totalRadius)
{
return;
}
findMaxSeparation(results2, polyB, xfB, polyA, xfA);
if (results2.separation > totalRadius)
{
return;
}
PolygonShape poly1; // reference polygon
PolygonShape poly2; // incident polygon
Transform xf1, xf2;
int edge1; // reference edge
bool flip;
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
if (results2.separation > k_relativeTol * results1.separation + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = results2.edgeIndex;
manifold.type = Manifold.ManifoldType.FACE_B;
flip = true;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = results1.edgeIndex;
manifold.type = Manifold.ManifoldType.FACE_A;
flip = false;
}
findIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.m_count;
Vec2[] vertices1 = poly1.m_vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
v11.set_Renamed(vertices1[iv1]);
v12.set_Renamed(vertices1[iv2]);
localTangent.set_Renamed(v12).subLocal(v11);
localTangent.normalize();
Vec2.crossToOutUnsafe(localTangent, 1f, localNormal); // Vec2 localNormal = Vec2.cross(dv,
// 1.0f);
planePoint.set_Renamed(v11).addLocal(v12).mulLocal(.5f); // Vec2 planePoint = 0.5f * (v11
// + v12);
Rot.mulToOutUnsafe(xf1.q, localTangent, tangent); // Vec2 sideNormal = Mul(xf1.R, v12
// - v11);
Vec2.crossToOutUnsafe(tangent, 1f, normal); // Vec2 frontNormal = Vec2.cross(sideNormal,
// 1.0f);
Transform.mulToOut(xf1, v11, v11);
Transform.mulToOut(xf1, v12, v12);
// v11 = Mul(xf1, v11);
// v12 = 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[2];
// ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
// np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
tangent.negateLocal();
np = clipSegmentToLine(clipPoints1, incidentEdge, tangent, sideOffset1, iv1);
tangent.negateLocal();
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = clipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal.set_Renamed(localNormal);
manifold.localPoint.set_Renamed(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];
Transform.mulTransToOut(xf2, clipPoints2[i].v, cp.localPoint);
// cp.m_localPoint = MulT(xf2, clipPoints2[i].v);
cp.id.set_Renamed(clipPoints2[i].id);
if (flip)
{
// Swap features
cp.id.flip();
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public override void evaluate(Manifold manifold, Transform xfA, Transform xfB)
{
pool.getCollision().collideEdgeAndPolygon(manifold, (EdgeShape)m_fixtureA.Shape, xfA, (PolygonShape)m_fixtureB.Shape, xfB);
}
/// <summary>
/// Creates this manifold as a copy of the other
/// </summary>
/// <param name="other"></param>
public Manifold(Manifold other)
{
points = new ManifoldPoint[Settings.maxManifoldPoints];
localNormal = other.localNormal.Clone();
localPoint = other.localPoint.Clone();
pointCount = other.pointCount;
type = other.type;
// djm: this is correct now
for (int i = 0; i < Settings.maxManifoldPoints; i++)
{
points[i] = new ManifoldPoint(other.points[i]);
}
}
public abstract void evaluate(Manifold manifold, Transform xfA, Transform xfB);
public override void Evaluate(Manifold manifold, Transform xfA, Transform xfB)
{
Pool.GetCollision().CollideEdgeAndPolygon(manifold, (EdgeShape)FixtureA.Shape, xfA, (PolygonShape)FixtureB.Shape, xfB);
}
public override void Evaluate(Manifold manifold, Transform xfA, Transform xfB)
{
Pool.GetCollision().CollideCircles(manifold, (CircleShape)FixtureA.Shape, xfA, (CircleShape)FixtureB.Shape, xfB);
}
