// 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);
}
/// <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();
}
}
/// <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 Shape Clone()
{
CircleShape shape = new CircleShape();
shape.m_p.set_Renamed(m_p);
shape.m_radius = m_radius;
return shape;
}
public override Shape Clone()
{
CircleShape shape = new CircleShape();
shape.P.Set(P);
shape.Radius = Radius;
return shape;
}
// 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();
}