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