/// Clipping for contact manifolds. public static int b2ClipSegmentToLine(b2ClipVertex[] vOut, b2ClipVertex[] vIn, b2Vec2 normal, float offset, byte vertexIndexA) { // Start with no output points int numOut = 0; // Calculate the distance of end points to the line float distance0 = b2Math.b2Dot(normal, vIn[0].v) - offset; float distance1 = b2Math.b2Dot(normal, vIn[1].v) - offset; // If the points are behind the plane if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; // If the points are on different sides of the plane if (distance0 * distance1 < 0.0f) { // Find intersection point of edge and plane float interp = distance0 / (distance0 - distance1); vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); // VertexA is hitting edgeB. vOut[numOut].id.indexA = vertexIndexA; vOut[numOut].id.indexB = vIn[0].id.indexB; vOut[numOut].id.typeA = b2ContactFeatureType.e_vertex; vOut[numOut].id.typeB = b2ContactFeatureType.e_face; ++numOut; } return numOut; }
public static void b2FindIncidentEdge(b2ClipVertex[] c, b2PolygonShape poly1, b2Transform xf1, int edge1, b2PolygonShape poly2, b2Transform xf2) { b2Vec2[] normals1 = poly1.Normals; int count2 = poly2.VertexCount; 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; }
/// 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 static void b2FindIncidentEdge(b2ClipVertex[] c, b2PolygonShape poly1, ref b2Transform xf1, int edge1, b2PolygonShape poly2, ref b2Transform xf2) { #if false b2Vec2[] normals1 = poly1.Normals; int count2 = poly2.VertexCount; 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(ref normal1, ref 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(ref xf2, ref 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(ref xf2, ref 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; #else var edge = poly1.Normals[edge1]; int count2 = poly2.VertexCount; b2Vec2[] vertices2 = poly2.Vertices; b2Vec2[] normals2 = poly2.Normals; // Get the normal of the reference edge in poly2's frame. float bx = xf1.q.c * edge.x - xf1.q.s * edge.y; float by = xf1.q.s * edge.x + xf1.q.c * edge.y; float normal1x = xf2.q.c * bx + xf2.q.s * by; float normal1y = -xf2.q.s * bx + xf2.q.c * by; // Find the incident edge on poly2. int index = 0; float minDot = b2Settings.b2_maxFloat; for (int i = 0; i < count2; ++i) { var normal = normals2[i]; float dot = normal1x * normal.x + normal1y * normal.y; 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; b2ClipVertex vertex; var vi1 = vertices2[i1]; var vi2 = vertices2[i2]; vertex.v.x = (xf2.q.c * vi1.x - xf2.q.s * vi1.y) + xf2.p.x; vertex.v.y = (xf2.q.s * vi1.x + xf2.q.c * vi1.y) + xf2.p.y; vertex.id.indexA = (byte)edge1; vertex.id.indexB = (byte)i1; vertex.id.typeA = b2ContactFeatureType.e_face; vertex.id.typeB = b2ContactFeatureType.e_vertex; c[0] = vertex; vertex.v.x = (xf2.q.c * vi2.x - xf2.q.s * vi2.y) + xf2.p.x; vertex.v.y = (xf2.q.s * vi2.x + xf2.q.c * vi2.y) + xf2.p.y; vertex.id.indexA = (byte)edge1; vertex.id.indexB = (byte)i2; vertex.id.typeA = b2ContactFeatureType.e_face; vertex.id.typeB = b2ContactFeatureType.e_vertex; c[1] = vertex; #endif }
/// Clipping for contact manifolds. public static int b2ClipSegmentToLine(b2ClipVertex[] vOut, b2ClipVertex[] vIn, ref b2Vec2 normal, float offset, byte vertexIndexA) { // Start with no output points int numOut = 0; var v0 = vIn[0].v; var v1 = vIn[1].v; // Calculate the distance of end points to the line float distance0 = normal.x * v0.x + normal.y * v0.y - offset; float distance1 = normal.x * v1.x + normal.y * v1.y - offset; // If the points are behind the plane if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; // If the points are on different sides of the plane if (distance0 * distance1 < 0.0f) { // Find intersection point of edge and plane float interp = distance0 / (distance0 - distance1); b2ClipVertex o; o.v.x = v0.x + interp * (v1.x - v0.x); o.v.y = v0.y + interp * (v1.y - v0.y); // VertexA is hitting edgeB. o.id.indexA = vertexIndexA; o.id.indexB = vIn[0].id.indexB; o.id.typeA = b2ContactFeatureType.e_vertex; o.id.typeB = b2ContactFeatureType.e_face; vOut[numOut] = o; ++numOut; } return numOut; }
// Algorithm: // 1. Classify v1 and v2 // 2. Classify polygon centroid as front or back // 3. Flip normal if necessary // 4. Initialize normal range to [-pi, pi] about face normal // 5. Adjust normal range according to adjacent edges // 6. Visit each separating axes, only accept axes within the range // 7. Return if _any_ axis indicates separation // 8. Clip public void Collide(ref b2Manifold manifold, b2EdgeShape edgeA, ref b2Transform xfA, b2PolygonShape polygonB, ref b2Transform xfB) { m_xf = b2Math.b2MulT(xfA, xfB); m_centroidB = b2Math.b2Mul(m_xf, polygonB.Centroid); m_v0 = edgeA.Vertex0; m_v1 = edgeA.Vertex1; m_v2 = edgeA.Vertex2; m_v3 = edgeA.Vertex3; bool hasVertex0 = edgeA.HasVertex0; bool hasVertex3 = edgeA.HasVertex3; b2Vec2 edge1 = m_v2 - m_v1; edge1.Normalize(); m_normal1.Set(edge1.y, -edge1.x); float offset1 = b2Math.b2Dot(m_normal1, m_centroidB - m_v1); float offset0 = 0.0f, offset2 = 0.0f; bool convex1 = false, convex2 = false; // Is there a preceding edge? if (hasVertex0) { b2Vec2 edge0 = m_v1 - m_v0; edge0.Normalize(); m_normal0.Set(edge0.y, -edge0.x); convex1 = b2Math.b2Cross(edge0, edge1) >= 0.0f; offset0 = b2Math.b2Dot(m_normal0, m_centroidB - m_v0); } // Is there a following edge? if (hasVertex3) { b2Vec2 edge2 = m_v3 - m_v2; edge2.Normalize(); m_normal2.Set(edge2.y, -edge2.x); convex2 = b2Math.b2Cross(edge1, edge2) > 0.0f; offset2 = b2Math.b2Dot(m_normal2, m_centroidB - 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 = m_normal1; m_lowerLimit = m_normal0; m_upperLimit = m_normal2; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = -m_normal1; } } else if (convex1) { m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal0; m_upperLimit = m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal2; m_upperLimit = -m_normal1; } } else if (convex2) { m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal1; m_upperLimit = m_normal2; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = -m_normal0; } } else { m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal1; m_upperLimit = m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal2; m_upperLimit = -m_normal0; } } } else if (hasVertex0) { if (convex1) { m_front = offset0 >= 0.0f || offset1 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal0; m_upperLimit = -m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = m_normal1; m_upperLimit = -m_normal1; } } else { m_front = offset0 >= 0.0f && offset1 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal1; m_upperLimit = -m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = m_normal1; m_upperLimit = -m_normal0; } } } else if (hasVertex3) { if (convex2) { m_front = offset1 >= 0.0f || offset2 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = m_normal2; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = m_normal1; } } else { m_front = offset1 >= 0.0f && offset2 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal2; m_upperLimit = m_normal1; } } } else { m_front = offset1 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = -m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = m_normal1; m_upperLimit = m_normal1; } } // Get polygonB in frameA m_polygonB = b2TempPolygon.Create(); m_polygonB.count = polygonB.VertexCount; for (int i = 0; i < polygonB.VertexCount; ++i) { m_polygonB.vertices[i] = b2Math.b2Mul(m_xf, polygonB.Vertices[i]); m_polygonB.normals[i] = b2Math.b2Mul(m_xf.q, polygonB.Normals[i]); } m_radius = 2.0f * b2Settings.b2_polygonRadius; manifold.pointCount = 0; b2EPAxis edgeAxis = ComputeEdgeSeparation(); // Console.WriteLine("b2EPAxis: {0} {1} {2}", edgeAxis.index, edgeAxis.separation, edgeAxis.type); // If no valid normal can be found than this edge should not collide. if (edgeAxis.type == b2EPAxisType.e_unknown) { return; } if (edgeAxis.separation > m_radius) { return; } b2EPAxis polygonAxis = ComputePolygonSeparation(); if (polygonAxis.type != b2EPAxisType.e_unknown && polygonAxis.separation > m_radius) { return; } // Use hysteresis for jitter reduction. const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; b2EPAxis primaryAxis; if (polygonAxis.type == b2EPAxisType.e_unknown) { primaryAxis = edgeAxis; } else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } b2ClipVertex[] ie = new b2ClipVertex[2]; b2ReferenceFace rf; if (primaryAxis.type == b2EPAxisType.e_edgeA) { manifold.type = b2ManifoldType.e_faceA; // Search for the polygon normal that is most anti-parallel to the edge normal. int bestIndex = 0; float bestValue = b2Math.b2Dot(m_normal, m_polygonB.normals[0]); for (int i = 1; i < m_polygonB.count; ++i) { float value = b2Math.b2Dot(m_normal, m_polygonB.normals[i]); if (value < bestValue) { bestValue = value; bestIndex = i; } } int i1 = bestIndex; int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; ie[0].v = m_polygonB.vertices[i1]; ie[0].id.indexA = 0; ie[0].id.indexB = (byte)i1; ie[0].id.typeA = b2ContactFeatureType.e_face; ie[0].id.typeB = b2ContactFeatureType.e_vertex; ie[1].v = m_polygonB.vertices[i2]; ie[1].id.indexA = 0; ie[1].id.indexB = (byte)i2; ie[1].id.typeA = b2ContactFeatureType.e_face; ie[1].id.typeB = b2ContactFeatureType.e_vertex; if (m_front) { rf.i1 = 0; rf.i2 = 1; rf.v1 = m_v1; rf.v2 = m_v2; rf.normal = m_normal1; } else { rf.i1 = 1; rf.i2 = 0; rf.v1 = m_v2; rf.v2 = m_v1; rf.normal = -m_normal1; } } else { manifold.type = b2ManifoldType.e_faceB; ie[0].v = m_v1; ie[0].id.indexA = 0; ie[0].id.indexB = (byte)primaryAxis.index; ie[0].id.typeA = b2ContactFeatureType.e_vertex; ie[0].id.typeB = b2ContactFeatureType.e_face; ie[1].v = m_v2; ie[1].id.indexA = 0; ie[1].id.indexB = (byte)primaryAxis.index; ie[1].id.typeA = b2ContactFeatureType.e_vertex; ie[1].id.typeB = b2ContactFeatureType.e_face; rf.i1 = primaryAxis.index; rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; rf.v1 = m_polygonB.vertices[rf.i1]; rf.v2 = m_polygonB.vertices[rf.i2]; rf.normal = m_polygonB.normals[rf.i1]; } rf.sideNormal1 = new b2Vec2(rf.normal.y, -rf.normal.x); rf.sideNormal2 = -rf.sideNormal1; rf.sideOffset1 = b2Math.b2Dot(rf.sideNormal1, rf.v1); rf.sideOffset2 = b2Math.b2Dot(rf.sideNormal2, rf.v2); // 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 = b2Collision.b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, (byte)rf.i1); if (np < b2Settings.b2_maxManifoldPoints) { return; } // Clip to negative box side 1 np = b2Collision.b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, (byte)rf.i2); if (np < b2Settings.b2_maxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.type == b2EPAxisType.e_edgeA) { manifold.localNormal = rf.normal; manifold.localPoint = rf.v1; } else { manifold.localNormal = polygonB.Normals[rf.i1]; manifold.localPoint = polygonB.Vertices[rf.i1]; } int pointCount = 0; for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i) { float separation; separation = b2Math.b2Dot(rf.normal, clipPoints2[i].v - rf.v1); if (separation <= m_radius) { b2ManifoldPoint cp = manifold.points[pointCount]; if (primaryAxis.type == b2EPAxisType.e_edgeA) { cp.localPoint = b2Math.b2MulT(m_xf, clipPoints2[i].v); cp.id = clipPoints2[i].id; } else { cp.localPoint = 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; } manifold.points[pointCount] = cp; ++pointCount; } } manifold.pointCount = pointCount; }
// Algorithm: // 1. Classify v1 and v2 // 2. Classify polygon centroid as front or back // 3. Flip normal if necessary // 4. Initialize normal range to [-pi, pi] about face normal // 5. Adjust normal range according to adjacent edges // 6. Visit each separating axes, only accept axes within the range // 7. Return if _any_ axis indicates separation // 8. Clip public void Collide(ref b2Manifold manifold, b2EdgeShape edgeA, ref b2Transform xfA, b2PolygonShape polygonB, ref b2Transform xfB) { m_xf = b2Math.b2MulT(xfA, xfB); m_centroidB = b2Math.b2Mul(m_xf, polygonB.Centroid); m_v0 = edgeA.Vertex0; m_v1 = edgeA.Vertex1; m_v2 = edgeA.Vertex2; m_v3 = edgeA.Vertex3; bool hasVertex0 = edgeA.HasVertex0; bool hasVertex3 = edgeA.HasVertex3; b2Vec2 edge1 = m_v2 - m_v1; edge1.Normalize(); m_normal1.Set(edge1.y, -edge1.x); b2Vec2 cenMinusV1 = m_centroidB - m_v1; float offset1 = b2Math.b2Dot(ref m_normal1, ref cenMinusV1); float offset0 = 0.0f, offset2 = 0.0f; bool convex1 = false, convex2 = false; // Is there a preceding edge? if (hasVertex0) { b2Vec2 edge0 = m_v1 - m_v0; edge0.Normalize(); m_normal0.Set(edge0.y, -edge0.x); convex1 = b2Math.b2Cross(ref edge0, ref edge1) >= 0.0f; b2Vec2 cenMinusV0 = m_centroidB - m_v0; offset0 = b2Math.b2Dot(ref m_normal0, ref cenMinusV0); } // Is there a following edge? if (hasVertex3) { b2Vec2 edge2 = m_v3 - m_v2; edge2.Normalize(); m_normal2.Set(edge2.y, -edge2.x); convex2 = b2Math.b2Cross(edge1, edge2) > 0.0f; offset2 = b2Math.b2Dot(m_normal2, m_centroidB - 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 = m_normal1; m_lowerLimit = m_normal0; m_upperLimit = m_normal2; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = -m_normal1; } } else if (convex1) { m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f); if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal0; m_upperLimit = m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal2; m_upperLimit = -m_normal1; } } else if (convex2) { m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f); if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal1; m_upperLimit = m_normal2; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = -m_normal0; } } else { m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal1; m_upperLimit = m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal2; m_upperLimit = -m_normal0; } } } else if (hasVertex0) { if (convex1) { m_front = offset0 >= 0.0f || offset1 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal0; m_upperLimit = -m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = m_normal1; m_upperLimit = -m_normal1; } } else { m_front = offset0 >= 0.0f && offset1 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = m_normal1; m_upperLimit = -m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = m_normal1; m_upperLimit = -m_normal0; } } } else if (hasVertex3) { if (convex2) { m_front = offset1 >= 0.0f || offset2 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = m_normal2; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = m_normal1; } } else { m_front = offset1 >= 0.0f && offset2 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = -m_normal2; m_upperLimit = m_normal1; } } } else { m_front = offset1 >= 0.0f; if (m_front) { m_normal = m_normal1; m_lowerLimit = -m_normal1; m_upperLimit = -m_normal1; } else { m_normal = -m_normal1; m_lowerLimit = m_normal1; m_upperLimit = m_normal1; } } // Get polygonB in frameA m_polygonB = b2TempPolygon.Create(); m_polygonB.count = polygonB.VertexCount; for (int i = 0; i < polygonB.VertexCount; ++i) { m_polygonB.vertices[i] = b2Math.b2Mul(m_xf, polygonB.Vertices[i]); m_polygonB.normals[i] = b2Math.b2Mul(m_xf.q, polygonB.Normals[i]); } m_radius = 2.0f * b2Settings.b2_polygonRadius; manifold.pointCount = 0; b2EPAxis edgeAxis = ComputeEdgeSeparation(); // Console.WriteLine("b2EPAxis: {0} {1} {2}", edgeAxis.index, edgeAxis.separation, edgeAxis.type); // If no valid normal can be found than this edge should not collide. if (edgeAxis.type == b2EPAxisType.e_unknown) { return; } if (edgeAxis.separation > m_radius) { return; } b2EPAxis polygonAxis = ComputePolygonSeparation(); if (polygonAxis.type != b2EPAxisType.e_unknown && polygonAxis.separation > m_radius) { return; } // Use hysteresis for jitter reduction. const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; b2EPAxis primaryAxis; if (polygonAxis.type == b2EPAxisType.e_unknown) { primaryAxis = edgeAxis; } else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } b2ClipVertex[] ie = new b2ClipVertex[2]; b2ReferenceFace rf; if (primaryAxis.type == b2EPAxisType.e_edgeA) { manifold.type = b2ManifoldType.e_faceA; // Search for the polygon normal that is most anti-parallel to the edge normal. int bestIndex = 0; float bestValue = b2Math.b2Dot(m_normal, m_polygonB.normals[0]); for (int i = 1; i < m_polygonB.count; ++i) { float value = b2Math.b2Dot(m_normal, m_polygonB.normals[i]); if (value < bestValue) { bestValue = value; bestIndex = i; } } int i1 = bestIndex; int i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0; ie[0].v = m_polygonB.vertices[i1]; ie[0].id.indexA = 0; ie[0].id.indexB = (byte)i1; ie[0].id.typeA = b2ContactFeatureType.e_face; ie[0].id.typeB = b2ContactFeatureType.e_vertex; ie[1].v = m_polygonB.vertices[i2]; ie[1].id.indexA = 0; ie[1].id.indexB = (byte)i2; ie[1].id.typeA = b2ContactFeatureType.e_face; ie[1].id.typeB = b2ContactFeatureType.e_vertex; if (m_front) { rf.i1 = 0; rf.i2 = 1; rf.v1 = m_v1; rf.v2 = m_v2; rf.normal = m_normal1; } else { rf.i1 = 1; rf.i2 = 0; rf.v1 = m_v2; rf.v2 = m_v1; rf.normal = -m_normal1; } } else { manifold.type = b2ManifoldType.e_faceB; ie[0].v = m_v1; ie[0].id.indexA = 0; ie[0].id.indexB = (byte)primaryAxis.index; ie[0].id.typeA = b2ContactFeatureType.e_vertex; ie[0].id.typeB = b2ContactFeatureType.e_face; ie[1].v = m_v2; ie[1].id.indexA = 0; ie[1].id.indexB = (byte)primaryAxis.index; ie[1].id.typeA = b2ContactFeatureType.e_vertex; ie[1].id.typeB = b2ContactFeatureType.e_face; rf.i1 = primaryAxis.index; rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0; rf.v1 = m_polygonB.vertices[rf.i1]; rf.v2 = m_polygonB.vertices[rf.i2]; rf.normal = m_polygonB.normals[rf.i1]; } rf.sideNormal1 = new b2Vec2(rf.normal.y, -rf.normal.x); rf.sideNormal2 = -rf.sideNormal1; rf.sideOffset1 = b2Math.b2Dot(rf.sideNormal1, rf.v1); rf.sideOffset2 = b2Math.b2Dot(rf.sideNormal2, rf.v2); // 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 = b2Collision.b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, (byte)rf.i1); if (np < b2Settings.b2_maxManifoldPoints) { return; } // Clip to negative box side 1 np = b2Collision.b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, (byte)rf.i2); if (np < b2Settings.b2_maxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.type == b2EPAxisType.e_edgeA) { manifold.localNormal = rf.normal; manifold.localPoint = rf.v1; } else { manifold.localNormal = polygonB.Normals[rf.i1]; manifold.localPoint = polygonB.Vertices[rf.i1]; } int pointCount = 0; for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i) { float separation; separation = b2Math.b2Dot(rf.normal, clipPoints2[i].v - rf.v1); if (separation <= m_radius) { b2ManifoldPoint cp = manifold.points[pointCount]; if (primaryAxis.type == b2EPAxisType.e_edgeA) { cp.localPoint = b2Math.b2MulT(m_xf, clipPoints2[i].v); cp.id = clipPoints2[i].id; } else { cp.localPoint = 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; } manifold.points[pointCount] = cp; ++pointCount; } } manifold.pointCount = pointCount; }