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); }