public void Set(b2ContactFeature cf)
{
indexA = cf.indexA;
indexB = cf.indexB;
typeA = cf.typeA;
typeB = cf.typeB;
}
public void Set(b2ContactFeature cf)
{
indexA = cf.indexA;
indexB = cf.indexB;
typeA = cf.typeA;
typeB = cf.typeB;
}
/// 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.
public static void b2GetPointStates(b2PointState[] state1, b2PointState[] state2,
ref b2Manifold manifold1, ref b2Manifold manifold2)
{
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
state1[i] = b2PointState.b2_nullState;
state2[i] = b2PointState.b2_nullState;
}
// Detect persists and removes.
for (int i = 0; i < manifold1.pointCount; ++i)
{
b2ContactFeature id = manifold1.points[i].id;
state1[i] = b2PointState.b2_removeState;
for (int j = 0; j < manifold2.pointCount; ++j)
{
if (manifold2.points[j].id.Equals(id))
{
state1[i] = b2PointState.b2_persistState;
break;
}
}
}
// Detect persists and adds.
for (int i = 0; i < manifold2.pointCount; ++i)
{
b2ContactFeature id = manifold2.points[i].id;
state2[i] = b2PointState.b2_addState;
for (int j = 0; j < manifold1.pointCount; ++j)
{
if (manifold1.points[j].id.Equals(id))
{
state2[i] = b2PointState.b2_persistState;
break;
}
}
}
}
/// Compute the collision manifold between an edge and a circle.
public static void b2CollideEdgeAndCircle(ref b2Manifold manifold,
b2EdgeShape edgeA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle in frame of edge
b2Vec2 Q = b2Math.b2MulT(xfA, b2Math.b2Mul(xfB, circleB.Position));
b2Vec2 A = edgeA.Vertex1, B = edgeA.Vertex2;
b2Vec2 e = B - A;
b2Vec2 diff;
// Barycentric coordinates
diff = B - Q;
float u = b2Math.b2Dot(ref e, ref diff); // B - Q);
diff = Q - A;
float v = b2Math.b2Dot(ref e, ref diff); // Q - A);
float radius = edgeA.Radius + circleB.Radius;
b2ContactFeature cf = b2ContactFeature.Zero;
cf.indexB = 0;
cf.typeB = b2ContactFeatureType.e_vertex;
// Region A
if (v <= 0.0f)
{
b2Vec2 P = A;
b2Vec2 d = Q - P;
float dd = d.LengthSquared; // b2Math.b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA.HasVertex0)
{
b2Vec2 A1 = edgeA.Vertex0;
b2Vec2 B1 = A;
b2Vec2 e1 = B1 - A1;
diff = B1 - Q;
float u1 = b2Math.b2Dot(ref e1, ref diff);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = b2ContactFeatureType.e_vertex;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
return;
}
// Region B
if (u <= 0.0f)
{
b2Vec2 P = B;
b2Vec2 d = Q - P;
float dd = d.LengthSquared; // b2Math.b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA.HasVertex3)
{
b2Vec2 B2 = edgeA.Vertex3;
b2Vec2 A2 = B;
b2Vec2 e2 = B2 - A2;
diff = Q - A2;
float v2 = b2Math.b2Dot(ref e2, ref diff);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = b2ContactFeatureType.e_vertex;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_circles;
manifold.localNormal.SetZero();
manifold.localPoint = P;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
return;
}
// Region AB
float den = e.Length; // b2Math.b2Dot(e, e);
System.Diagnostics.Debug.Assert(den > 0.0f);
b2Vec2 xP = (1.0f / den) * (u * A + v * B);
b2Vec2 xd = Q - xP;
float xdd = xd.LengthSquared; // b2Math.b2Dot(xd, xd);
if (xdd > radius * radius)
{
return;
}
b2Vec2 n = b2Vec2.Zero; // new b2Vec2(-e.y, e.x);
n.m_x = -e.y;
n.m_y = e.x;
diff = Q - A;
if (b2Math.b2Dot(ref n, ref diff) < 0.0f)
{
// n.Set(-n.x, -n.y);
n.Set(-n.m_x, -n.m_y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = b2ContactFeatureType.e_face;
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = n;
manifold.localPoint = A;
manifold.points[0].id.key = 0;
manifold.points[0].id.Set(cf);
manifold.points[0].localPoint = circleB.Position;
}
/// Compute the collision manifold between two polygons.
// 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
public static void b2CollidePolygons(ref b2Manifold manifold,
b2PolygonShape polyA, ref b2Transform xfA,
b2PolygonShape polyB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA = 0;
float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
return;
int edgeB = 0;
float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
return;
b2PolygonShape poly1; // reference polygon
b2PolygonShape poly2; // incident polygon
b2Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.type = b2ManifoldType.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = b2ManifoldType.e_faceA;
flip = 0;
}
b2ClipVertex[] incidentEdge = new b2ClipVertex[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
b2Vec2[] vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent = v12 - v11;
localTangent.Normalize();
b2Vec2 localNormal = localTangent.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
b2Vec2 tangent = b2Math.b2Mul(xf1.q, localTangent);
b2Vec2 normal = tangent.UnitCross(); // b2Math.b2Cross(tangent, 1.0f);
v11 = b2Math.b2Mul(xf1, v11);
v12 = b2Math.b2Mul(xf1, v12);
// Face offset.
float frontOffset = b2Math.b2Dot(ref normal, ref v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -b2Math.b2Dot(ref tangent, ref v11) + totalRadius;
float sideOffset2 = b2Math.b2Dot(ref tangent, ref v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, (byte)iv1);
if (np < 2)
return;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, (byte)iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal = localNormal;
manifold.localPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
float separation = b2Math.b2Dot(ref normal, ref clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
cp.localPoint = b2Math.b2MulT(xf2, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
if (flip != 0)
{
// Swap features
b2ContactFeature cf = cp.id;
cp.id.indexA = cf.indexB;
cp.id.indexB = cf.indexA;
cp.id.typeA = cf.typeB;
cp.id.typeB = cf.typeA;
}
manifold.points[pointCount] = cp;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public bool Equals(ref b2ContactFeature bcf)
{
return (indexA == bcf.indexA && indexB == bcf.indexB && typeA == bcf.typeA && typeB == bcf.typeB);
}
public bool Equals(ref b2ContactFeature bcf)
{
return(indexA == bcf.indexA && indexB == bcf.indexB && typeA == bcf.typeA && typeB == bcf.typeB);
}
public override bool Equals(object o)
{
b2ContactFeature bcf = (b2ContactFeature)o;
return(indexA == bcf.indexA && indexB == bcf.indexB && typeA == bcf.typeA && typeB == bcf.typeB);
}
public static void b2CollidePolygons(b2Manifold manifold, b2PolygonShape polyA, ref b2Transform xfA, b2PolygonShape polyB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA = 0;
float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
{
return;
}
b2PolygonShape poly1; // reference polygon
b2PolygonShape poly2; // incident polygon
b2Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.type = b2ManifoldType.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = b2ManifoldType.e_faceA;
flip = 0;
}
b2ClipVertex[] incidentEdge = _incidentEdge;
b2FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1.VertexCount;
b2Vec2[] vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent;
localTangent.x = v12.x - v11.x;
localTangent.y = v12.y - v11.y;
localTangent.Normalize();
b2Vec2 localNormal;
localNormal.x = localTangent.y; //.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f);
localNormal.y = -localTangent.x;
b2Vec2 planePoint;
planePoint.x = 0.5f * (v11.x + v12.x);
planePoint.y = 0.5f * (v11.y + v12.y);
b2Vec2 tangent;
tangent.x = xf1.q.c * localTangent.x - xf1.q.s * localTangent.y;
tangent.y = xf1.q.s * localTangent.x + xf1.q.c * localTangent.y;
float normalx = tangent.y; //UnitCross(); // b2Math.b2Cross(tangent, 1.0f);
float normaly = -tangent.x;
float v11x = (xf1.q.c * v11.x - xf1.q.s * v11.y) + xf1.p.x;
float v11y = (xf1.q.s * v11.x + xf1.q.c * v11.y) + xf1.p.y;
float v12x = (xf1.q.c * v12.x - xf1.q.s * v12.y) + xf1.p.x;
float v12y = (xf1.q.s * v12.x + xf1.q.c * v12.y) + xf1.p.y;
// Face offset.
float frontOffset = normalx * v11x + normaly * v11y;
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -(tangent.x * v11x + tangent.y * v11y) + totalRadius;
float sideOffset2 = tangent.x * v12x + tangent.y * v12y + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = _clipPoints1;
int np;
// Clip to box side 1
b2Vec2 t;
t.x = -tangent.x;
t.y = -tangent.y;
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, ref t, sideOffset1, (byte)iv1);
if (np < 2)
{
return;
}
b2ClipVertex[] clipPoints2 = _clipPoints2;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref tangent, sideOffset2, (byte)iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal = localNormal;
manifold.localPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
var v = clipPoints2[i].v;
//float separation = b2Math.b2Dot(ref normal, ref v) - frontOffset;
float separation = normalx * v.x + normaly * v.y - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
//cp.localPoint = b2Math.b2MulT(ref xf2, ref v);
float px = v.x - xf2.p.x;
float py = v.y - xf2.p.y;
cp.localPoint.x = (xf2.q.c * px + xf2.q.s * py);
cp.localPoint.y = (-xf2.q.s * px + xf2.q.c * py);
cp.id = clipPoints2[i].id;
if (flip != 0)
{
// Swap features
b2ContactFeature cf = cp.id;
cp.id.indexA = cf.indexB;
cp.id.indexB = cf.indexA;
cp.id.typeA = cf.typeB;
cp.id.typeB = cf.typeA;
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}