//public static int GJKIterations; // GJK using Voronoi regions (Christer Ericson) and region selection // optimizations (Casey Muratori). // The origin is either in the region of points[1] or in the edge region. The origin is // not in region of points[0] because that is the old point. public static int ProcessTwo(out Vector2 x1, out Vector2 x2, ref Vector2[] p1s, ref Vector2[] p2s, ref Vector2[] points) { // If in point[1] region Vector2 r = -points[1]; Vector2 d = points[0] - points[1]; float length = CommonMath.Normalize(ref d); float lambda = Vector2.Dot(r, d); if (lambda <= 0.0f || length < Settings.FLT_EPSILON) { // The simplex is reduced to a point. x1 = p1s[1]; x2 = p2s[1]; p1s[0] = p1s[1]; p2s[0] = p2s[1]; points[0] = points[1]; return(1); } // Else in edge region lambda /= length; x1 = p1s[1] + lambda * (p1s[0] - p1s[1]); x2 = p2s[1] + lambda * (p2s[0] - p2s[1]); return(2); }
public static float DistanceCC(out Vector2 x1, out Vector2 x2, CircleShape circle1, XForm xf1, CircleShape circle2, XForm xf2) { Vector2 p1 = CommonMath.Mul(xf1, circle1.GetLocalPosition()); Vector2 p2 = CommonMath.Mul(xf2, circle2.GetLocalPosition()); Vector2 d = p2 - p1; float dSqr = Vector2.Dot(d, d); float r1 = circle1.GetRadius() - Settings.ToiSlop; float r2 = circle2.GetRadius() - Settings.ToiSlop; float r = r1 + r2; if (dSqr > r * r) { float dLen = CommonMath.Normalize(ref d); float distance = dLen - r; x1 = p1 + r1 * d; x2 = p2 - r2 * d; return(distance); } else if (dSqr > Settings.FLT_EPSILON * Settings.FLT_EPSILON) { d.Normalize(); x1 = p1 + r1 * d; x2 = x1; return(0.0f); } x1 = p1; x2 = x1; return(0.0f); }
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. Vector2 c = CommonMath.Mul(xf2, circle.GetLocalPosition()); Vector2 cLocal = CommonMath.MulT(xf1, c); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.FLT_MAX; float radius = circle.GetRadius(); int vertexCount = polygon.VertexCount; Vector2[] vertices = polygon.GetVertices(); Vector2[] normals = polygon.Normals; for (int i = 0; i < vertexCount; ++i) { float s = Vector2.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 < Settings.FLT_EPSILON) { manifold.PointCount = 1; manifold.Normal = CommonMath.Mul(xf1.R, normals[normalIndex]); manifold.Points[0].ID.Features.IncidentEdge = (byte)normalIndex; manifold.Points[0].ID.Features.IncidentVertex = NullFeature; manifold.Points[0].ID.Features.ReferenceEdge = 0; manifold.Points[0].ID.Features.Flip = 0; Vector2 position = c - radius * manifold.Normal; manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, position); manifold.Points[0].LocalPoint2 = CommonMath.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; Vector2 e = vertices[vertIndex2] - vertices[vertIndex1]; float length = CommonMath.Normalize(ref e); //Box2DXDebug.Assert(length > Settings.FLT_EPSILON); // Project the center onto the edge. float u = Vector2.Dot(cLocal - vertices[vertIndex1], e); Vector2 p; if (u <= 0.0f) { p = vertices[vertIndex1]; manifold.Points[0].ID.Features.IncidentEdge = NullFeature; manifold.Points[0].ID.Features.IncidentVertex = (byte)vertIndex1; } else if (u >= length) { p = vertices[vertIndex2]; manifold.Points[0].ID.Features.IncidentEdge = 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 = NullFeature; } Vector2 d = cLocal - p; float dist = CommonMath.Normalize(ref d); if (dist > radius) { return; } manifold.PointCount = 1; manifold.Normal = CommonMath.Mul(xf1.R, d); Vector2 position_ = c - radius * manifold.Normal; manifold.Points[0].LocalPoint1 = CommonMath.MulT(xf1, position_); manifold.Points[0].LocalPoint2 = CommonMath.MulT(xf2, position_); manifold.Points[0].Separation = dist - radius; manifold.Points[0].ID.Features.ReferenceEdge = 0; manifold.Points[0].ID.Features.Flip = 0; }