public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2) { int count1 = poly1.VertexCount; Vec2[] normals1 = poly1.Normals; int count2 = poly2.VertexCount; Vec2[] vertices2 = poly2.GetVertices(); Vec2[] normals2 = poly2.Normals; Box2DXDebug.Assert(0 <= edge1 && edge1 < count1); // Get the normal of the reference edge in poly2's frame. Vec2 normal1 = Common.Math.MulT(xf2.R, Common.Math.Mul(xf1.R, normals1[edge1])); // Find the incident edge on poly2. int index = 0; float minDot = Settings.FLT_MAX; for (int i = 0; i < count2; ++i) { float dot = Vec2.Dot(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 = new ClipVertex[2]; c[0].V = Common.Math.Mul(xf2, vertices2[i1]); c[0].ID.Features.ReferenceEdge = (byte)edge1; c[0].ID.Features.IncidentEdge = (byte)i1; c[0].ID.Features.IncidentVertex = 0; c[1].V = Common.Math.Mul(xf2, vertices2[i2]); c[1].ID.Features.ReferenceEdge = (byte)edge1; c[1].ID.Features.IncidentEdge = (byte)i2; c[1].ID.Features.IncidentVertex = 1; }
/// <summary> /// Find the separation between poly1 and poly2 for a give edge normal on poly1. /// </summary> /// <param name="poly1"></param> /// <param name="xf1"></param> /// <param name="edge1"></param> /// <param name="poly2"></param> /// <param name="xf2"></param> /// <returns></returns> public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2) { int count1 = poly1.VertexCount; Vec2[] vertices1 = poly1.GetVertices(); Vec2[] normals1 = poly1.Normals; int count2 = poly2.VertexCount; Vec2[] vertices2 = poly2.GetVertices(); Box2DXDebug.Assert(0 <= edge1 && edge1 < count1); // Convert normal from poly1's frame into poly2's frame. Vec2 normal1World = Common.Math.Mul(xf1.R, normals1[edge1]); Vec2 normal1 = Common.Math.MulT(xf2.R, normal1World); // Find support vertex on poly2 for -normal. int index = 0; float minDot = Common.Settings.FLT_MAX; for (int i = 0; i < count2; ++i) { float dot = Vec2.Dot(vertices2[i], normal1); if (dot < minDot) { minDot = dot; index = i; } } Vec2 v1 = Common.Math.Mul(xf1, vertices1[edge1]); Vec2 v2 = Common.Math.Mul(xf2, vertices2[index]); float separation = Vec2.Dot(v2 - v1, normal1World); return(separation); }
/// <summary> /// Find the separation between poly1 and poly2 for a give edge normal on poly1. /// </summary> /// <param name="poly1"></param> /// <param name="xf1"></param> /// <param name="edge1"></param> /// <param name="poly2"></param> /// <param name="xf2"></param> /// <returns></returns> public static float EdgeSeparation(PolygonShape poly1, XForm xf1, int edge1, PolygonShape poly2, XForm xf2) { int count1 = poly1.VertexCount; Vec2[] vertices1 = poly1.GetVertices(); Vec2[] normals1 = poly1.Normals; int count2 = poly2.VertexCount; Vec2[] vertices2 = poly2.GetVertices(); Box2DXDebug.Assert(0 <= edge1 && edge1 < count1); // Convert normal from poly1's frame into poly2's frame. Vec2 normal1World = Common.Math.Mul(xf1.R, normals1[edge1]); Vec2 normal1 = Common.Math.MulT(xf2.R, normal1World); // Find support vertex on poly2 for -normal. int index = 0; float minDot = Common.Settings.FLT_MAX; for (int i = 0; i < count2; ++i) { float dot = Vec2.Dot(vertices2[i], normal1); if (dot < minDot) { minDot = dot; index = i; } } Vec2 v1 = Common.Math.Mul(xf1, vertices1[edge1]); Vec2 v2 = Common.Math.Mul(xf2, vertices2[index]); float separation = Vec2.Dot(v2 - v1, normal1World); return separation; }
public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygon, XForm xf1, CircleShape circle, XForm xf2) { manifold.PointCount = 0; // Compute circle position in the frame of the polygon. Vec2 c = Common.Math.Mul(xf2, circle.GetLocalPosition()); Vec2 cLocal = Common.Math.MulT(xf1, c); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.FLT_MAX; float radius = circle.GetRadius(); int vertexCount = polygon.VertexCount; Vec2[] vertices = polygon.GetVertices(); Vec2[] normals = polygon.Normals; for (int i = 0; i < vertexCount; ++i) { float s = Vec2.Dot(normals[i], cLocal - vertices[i]); if (s > radius) { // Early out. return; } if (s > separation) { separation = s; normalIndex = i; } } // If the center is inside the polygon ... if (separation < Common.Settings.FLT_EPSILON) { manifold.PointCount = 1; manifold.Normal = Common.Math.Mul(xf1.R, normals[normalIndex]); manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex; manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature; manifold.Points[0].ID.Features.ReferenceEdge = 0; manifold.Points[0].ID.Features.Flip = 0; Vec2 position = c - radius * manifold.Normal; manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position); manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position); manifold.Points[0].Separation = separation - radius; return; } // Project the circle center onto the edge segment. int vertIndex1 = normalIndex; int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; Vec2 e = vertices[vertIndex2] - vertices[vertIndex1]; float length = e.Normalize(); Box2DXDebug.Assert(length > Settings.FLT_EPSILON); // Project the center onto the edge. float u = Vec2.Dot(cLocal - vertices[vertIndex1], e); Vec2 p; if (u <= 0.0f) { p = vertices[vertIndex1]; manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature; manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1; } else if (u >= length) { p = vertices[vertIndex2]; manifold.Points[0].ID.Features.IncidentEdge = Collision.NullFeature; manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex2; } else { p = vertices[vertIndex1] + u * e; manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex; manifold.Points[0].ID.Features.IncidentVertex = Collision.NullFeature; } Vec2 d = cLocal - p; float dist = d.Normalize(); if (dist > radius) { return; } manifold.PointCount = 1; manifold.Normal = Common.Math.Mul(xf1.R, d); Vec2 position_ = c - radius * manifold.Normal; manifold.Points[0].LocalPoint1 = Common.Math.MulT(xf1, position_); manifold.Points[0].LocalPoint2 = Common.Math.MulT(xf2, position_); manifold.Points[0].Separation = dist - radius; manifold.Points[0].ID.Features.ReferenceEdge = 0; manifold.Points[0].ID.Features.Flip = 0; }