public static float b2EdgeSeparation(b2PolygonShape poly1, b2Transform xf1, int edge1,
b2PolygonShape poly2, b2Transform xf2)
{
b2Vec2[] vertices1 = poly1.Vertices;
b2Vec2[] normals1 = poly1.Normals;
int count2 = poly2.GetVertexCount();
b2Vec2[] vertices2 = poly2.Vertices;
// Convert normal from poly1's frame into poly2's frame.
b2Vec2 normal1World = b2Math.b2Mul(xf1.q, normals1[edge1]);
b2Vec2 normal1 = b2Math.b2MulT(xf2.q, normal1World);
// Find support vertex on poly2 for -normal.
int index = 0;
float minDot = b2Settings.b2_maxFloat;
for (int i = 0; i < count2; ++i)
{
float dot = b2Math.b2Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
b2Vec2 v1 = b2Math.b2Mul(xf1, vertices1[edge1]);
b2Vec2 v2 = b2Math.b2Mul(xf2, vertices2[index]);
float separation = b2Math.b2Dot(v2 - v1, normal1World);
return(separation);
}
public static void b2FindIncidentEdge(b2ClipVertex[] c,
b2PolygonShape poly1, b2Transform xf1, int edge1,
b2PolygonShape poly2, b2Transform xf2)
{
b2Vec2[] normals1 = poly1.Normals;
int count2 = poly2.GetVertexCount();
b2Vec2[] vertices2 = poly2.Vertices;
b2Vec2[] normals2 = poly2.Normals;
// Get the normal of the reference edge in poly2's frame.
b2Vec2 normal1 = b2Math.b2MulT(xf2.q, b2Math.b2Mul(xf1.q, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = b2Settings.b2_maxFloat;
for (int i = 0; i < count2; ++i)
{
float dot = b2Math.b2Dot(normal1, normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c[0].v = b2Math.b2Mul(xf2, vertices2[i1]);
c[0].id.indexA = (byte)edge1;
c[0].id.indexB = (byte)i1;
c[0].id.typeA = b2ContactFeatureType.e_face;
c[0].id.typeB = b2ContactFeatureType.e_vertex;
c[1].v = b2Math.b2Mul(xf2, vertices2[i2]);
c[1].id.indexA = (byte)edge1;
c[1].id.indexB = (byte)i2;
c[1].id.typeA = b2ContactFeatureType.e_face;
c[1].id.typeB = b2ContactFeatureType.e_vertex;
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
public static float b2FindMaxSeparation(out int edgeIndex,
b2PolygonShape poly1, b2Transform xf1,
b2PolygonShape poly2, b2Transform xf2)
{
int count1 = poly1.GetVertexCount();
b2Vec2[] normals1 = poly1.Normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
b2Vec2 d = b2Math.b2Mul(xf2, poly2.Centroid) - b2Math.b2Mul(xf1, poly1.Centroid);
b2Vec2 dLocal1 = b2Math.b2MulT(xf1.q, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -b2Settings.b2_maxFloat;
for (int i = 0; i < count1; ++i)
{
float dot = b2Math.b2Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
edgeIndex = edge;
return(s);
}
// Perform a local search for the best edge normal.
for (; ;)
{
if (increment == -1)
{
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
}
else
{
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
}
s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return(bestSeparation);
}
/// Compute the collision manifold between a polygon and a circle.
public static void b2CollidePolygonAndCircle(b2Manifold manifold,
b2PolygonShape polygonA, b2Transform xfA,
b2CircleShape circleB, b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c = b2Math.b2Mul(xfB, circleB.m_p);
b2Vec2 cLocal = b2Math.b2MulT(xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -b2Settings.b2_maxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.GetVertexCount();
b2Vec2[] vertices = polygonA.Vertices;
b2Vec2[] normals = polygonA.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = b2Math.b2Dot(normals[i], cLocal - vertices[i]);
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;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2Settings.b2_epsilon)
{
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint = 0.5f * (v1 + v2);
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
return;
}
// Compute barycentric coordinates
float u1 = b2Math.b2Dot(cLocal - v1, v2 - v1);
float u2 = b2Math.b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else if (u2 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v2;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else
{
b2Vec2 faceCenter = 0.5f * (v1 + v2);
separation = b2Math.b2Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.m_p;
manifold.points[0].id = b2ContactFeature.Zero;
}
}
public virtual void Dump(int bodyIndex)
{
System.Diagnostics.Debug.WriteLine(" b2FixtureDef fd;");
System.Diagnostics.Debug.WriteLine(" fd.friction = {0:N5};", m_friction);
System.Diagnostics.Debug.WriteLine(" fd.restitution = {0:N5};", m_restitution);
System.Diagnostics.Debug.WriteLine(" fd.density = {0:N5};", m_density);
System.Diagnostics.Debug.WriteLine(" fd.isSensor = bool({0});", m_isSensor);
System.Diagnostics.Debug.WriteLine(" fd.filter.categoryBits = uint16({0});", m_filter.categoryBits);
System.Diagnostics.Debug.WriteLine(" fd.filter.maskBits = uint16({0});", m_filter.maskBits);
System.Diagnostics.Debug.WriteLine(" fd.filter.groupIndex = int16({0});", m_filter.groupIndex);
switch (m_shape.ShapeType)
{
case b2ShapeType.e_circle:
{
b2CircleShape s = (b2CircleShape)m_shape;
System.Diagnostics.Debug.WriteLine(" b2CircleShape shape;");
System.Diagnostics.Debug.WriteLine(" shape.m_radius = {0:N5};", s.Radius);
System.Diagnostics.Debug.WriteLine(" shape.m_p.Set({0:N5}, {0:N5});", s.Position.x, s.Position.y);
}
break;
case b2ShapeType.e_edge:
{
b2EdgeShape s = (b2EdgeShape)m_shape;
System.Diagnostics.Debug.WriteLine(" b2EdgeShape shape;");
System.Diagnostics.Debug.WriteLine(" shape.m_radius = {0:N5};", s.Radius);
System.Diagnostics.Debug.WriteLine(" shape.m_vertex0.Set({0:N5}, {0:N5});", s.Vertex0.x, s.Vertex0.y);
System.Diagnostics.Debug.WriteLine(" shape.m_vertex1.Set({0:N5}, {0:N5});", s.Vertex1.x, s.Vertex1.y);
System.Diagnostics.Debug.WriteLine(" shape.m_vertex2.Set({0:N5}, {0:N5});", s.Vertex2.x, s.Vertex2.y);
System.Diagnostics.Debug.WriteLine(" shape.m_vertex3.Set({0:N5}, {0:N5});", s.Vertex3.x, s.Vertex3.y);
System.Diagnostics.Debug.WriteLine(" shape.m_hasVertex0 = bool({0});", s.HasVertex0);
System.Diagnostics.Debug.WriteLine(" shape.m_hasVertex3 = bool({0});", s.HasVertex3);
}
break;
case b2ShapeType.e_polygon:
{
b2PolygonShape s = (b2PolygonShape)m_shape;
System.Diagnostics.Debug.WriteLine(" b2PolygonShape shape;");
System.Diagnostics.Debug.WriteLine(" b2Vec2 vs[{0}];", b2Settings.b2_maxPolygonVertices);
for (int i = 0; i < s.GetVertexCount(); ++i)
{
System.Diagnostics.Debug.WriteLine(" vs[{0}].Set({0:N5}, {0:N5});", i, s.Vertices[i].x, s.Vertices[i].y);
}
System.Diagnostics.Debug.WriteLine(" shape.Set(vs, {0});", s.GetVertexCount());
}
break;
case b2ShapeType.e_chain:
{
b2ChainShape s = (b2ChainShape)m_shape;
System.Diagnostics.Debug.WriteLine(" b2ChainShape shape;");
System.Diagnostics.Debug.WriteLine(" b2Vec2 vs[{0}];", s.Count);
for (int i = 0; i < s.Count; ++i)
{
System.Diagnostics.Debug.WriteLine(" vs[{0}].Set({0:N5}, {0:N5});", i, s.Vertices[i].x, s.Vertices[i].y);
}
System.Diagnostics.Debug.WriteLine(" shape.CreateChain(vs, {0});", s.Count);
System.Diagnostics.Debug.WriteLine(" shape.m_prevVertex.Set({0:N5}, {0:N5});", s.PrevVertex.x, s.PrevVertex.y);
System.Diagnostics.Debug.WriteLine(" shape.m_nextVertex.Set({0:N5}, {0:N5});", s.NextVertex.x, s.NextVertex.y);
System.Diagnostics.Debug.WriteLine(" shape.m_hasPrevVertex = bool({0});", s.HasPrevVertex);
System.Diagnostics.Debug.WriteLine(" shape.m_hasNextVertex = bool({0});", s.HasNextVertex);
}
break;
default:
return;
}
System.Diagnostics.Debug.WriteLine("");
System.Diagnostics.Debug.WriteLine(" fd.shape = &shape;");
System.Diagnostics.Debug.WriteLine("");
System.Diagnostics.Debug.WriteLine(" bodies[{0}].CreateFixture(&fd);", bodyIndex);
}