/// <summary> /// Get the contact manifold. Do not modify the manifold unless you understand the /// internals of Box2D. /// </summary> /// <param name="manifold">The manifold.</param> public void GetManifold(out Manifold manifold) { manifold = Manifold; }
/// <summary> /// Evaluate this contact with your own manifold and transforms. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="transformA">The first transform.</param> /// <param name="transformB">The second transform.</param> private void Evaluate(ref Manifold manifold, ref Transform transformA, ref Transform transformB) { switch (_type) { case ContactType.Polygon: Collision.Collision.CollidePolygons(ref manifold, (PolygonShape)FixtureA.Shape, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB); break; case ContactType.PolygonAndCircle: Collision.Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); break; case ContactType.EdgeAndCircle: Collision.Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); break; case ContactType.EdgeAndPolygon: Collision.Collision.CollideEdgeAndPolygon(ref manifold, (EdgeShape)FixtureA.Shape, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB); break; case ContactType.LoopAndCircle: LoopShape loop = (LoopShape)FixtureA.Shape; loop.GetChildEdge(ref _edge, ChildIndexA); Collision.Collision.CollideEdgeAndCircle(ref manifold, _edge, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); break; case ContactType.LoopAndPolygon: LoopShape loop2 = (LoopShape)FixtureA.Shape; loop2.GetChildEdge(ref _edge, ChildIndexA); Collision.Collision.CollideEdgeAndPolygon(ref manifold, _edge, ref transformA, (PolygonShape)FixtureB.Shape, ref transformB); break; case ContactType.Circle: Collision.Collision.CollideCircles(ref manifold, (CircleShape)FixtureA.Shape, ref transformA, (CircleShape)FixtureB.Shape, ref transformB); break; } }
/// <summary> /// Compute contact points for edge versus circle. /// This accounts for edge connectivity. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="edgeA">The edge A.</param> /// <param name="transformA">The transform A.</param> /// <param name="circleB">The circle B.</param> /// <param name="transformB">The transform B.</param> public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, ref Transform transformA, CircleShape circleB, ref Transform transformB) { manifold.PointCount = 0; // Compute circle in frame of edge Vector2 Q = MathUtils.MultiplyT(ref transformA, MathUtils.Multiply(ref transformB, ref circleB._position)); Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2; Vector2 e = B - A; // Barycentric coordinates float u = Vector2.Dot(e, B - Q); float v = Vector2.Dot(e, Q - A); float radius = edgeA.Radius + circleB.Radius; ContactFeature cf; cf.IndexB = 0; cf.TypeB = (byte)ContactFeatureType.Vertex; Vector2 P, d; // Region A if (v <= 0.0f) { P = A; d = Q - P; float dd; Vector2.Dot(ref d, ref d, out dd); if (dd > radius * radius) { return; } // Is there an edge connected to A? if (edgeA.HasVertex0) { Vector2 A1 = edgeA.Vertex0; Vector2 B1 = A; Vector2 e1 = B1 - A1; float u1 = Vector2.Dot(e1, B1 - Q); // Is the circle in Region AB of the previous edge? if (u1 > 0.0f) { return; } } cf.IndexA = 0; cf.TypeA = (byte)ContactFeatureType.Vertex; manifold.PointCount = 1; manifold.Type = ManifoldType.Circles; manifold.LocalNormal = Vector2.Zero; manifold.LocalPoint = P; ManifoldPoint mp = new ManifoldPoint(); mp.Id.Key = 0; mp.Id.Features = cf; mp.LocalPoint = circleB.Position; manifold.Points[0] = mp; return; } // Region B if (u <= 0.0f) { P = B; d = Q - P; float dd; Vector2.Dot(ref d, ref d, out dd); if (dd > radius * radius) { return; } // Is there an edge connected to B? if (edgeA.HasVertex3) { Vector2 B2 = edgeA.Vertex3; Vector2 A2 = B; Vector2 e2 = B2 - A2; float v2 = Vector2.Dot(e2, Q - A2); // Is the circle in Region AB of the next edge? if (v2 > 0.0f) { return; } } cf.IndexA = 1; cf.TypeA = (byte)ContactFeatureType.Vertex; manifold.PointCount = 1; manifold.Type = ManifoldType.Circles; manifold.LocalNormal = Vector2.Zero; manifold.LocalPoint = P; ManifoldPoint mp = new ManifoldPoint(); mp.Id.Key = 0; mp.Id.Features = cf; mp.LocalPoint = circleB.Position; manifold.Points[0] = mp; return; } // Region AB float den; Vector2.Dot(ref e, ref e, out den); Debug.Assert(den > 0.0f); P = (1.0f / den) * (u * A + v * B); d = Q - P; float dd2; Vector2.Dot(ref d, ref d, out dd2); if (dd2 > radius * radius) { return; } Vector2 n = new Vector2(-e.Y, e.X); if (Vector2.Dot(n, Q - A) < 0.0f) { n = new Vector2(-n.X, -n.Y); } n.Normalize(); cf.IndexA = 0; cf.TypeA = (byte)ContactFeatureType.Face; manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = n; manifold.LocalPoint = A; ManifoldPoint mp2 = new ManifoldPoint(); mp2.Id.Key = 0; mp2.Id.Features = cf; mp2.LocalPoint = circleB.Position; manifold.Points[0] = mp2; }
/// <summary> /// Collides and edge and a polygon, taking into account edge adjacency. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="edgeA">The edge A.</param> /// <param name="xfA">The xf A.</param> /// <param name="polygonB">The polygon B.</param> /// <param name="xfB">The xf B.</param> public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) { MathUtils.MultiplyT(ref xfA, ref xfB, out _xf); // Edge geometry _edgeA.V0 = edgeA.Vertex0; _edgeA.V1 = edgeA.Vertex1; _edgeA.V2 = edgeA.Vertex2; _edgeA.V3 = edgeA.Vertex3; Vector2 e = _edgeA.V2 - _edgeA.V1; // Normal points outwards in CCW order. _edgeA.Normal = new Vector2(e.Y, -e.X); _edgeA.Normal.Normalize(); _edgeA.HasVertex0 = edgeA.HasVertex0; _edgeA.HasVertex3 = edgeA.HasVertex3; // Proxy for edge _proxyA.Vertices[0] = _edgeA.V1; _proxyA.Vertices[1] = _edgeA.V2; _proxyA.Normals[0] = _edgeA.Normal; _proxyA.Normals[1] = -_edgeA.Normal; _proxyA.Centroid = 0.5f * (_edgeA.V1 + _edgeA.V2); _proxyA.Count = 2; // Proxy for polygon _proxyB.Count = polygonB.Vertices.Count; _proxyB.Centroid = MathUtils.Multiply(ref _xf, ref polygonB.MassData.Centroid); for (int i = 0; i < polygonB.Vertices.Count; ++i) { _proxyB.Vertices[i] = MathUtils.Multiply(ref _xf, polygonB.Vertices[i]); _proxyB.Normals[i] = MathUtils.Multiply(ref _xf.R, polygonB.Normals[i]); } _radius = 2.0f * Settings.PolygonRadius; _limit11 = Vector2.Zero; _limit12 = Vector2.Zero; _limit21 = Vector2.Zero; _limit22 = Vector2.Zero; //Collide(ref manifold); inline start manifold.PointCount = 0; //ComputeAdjacency(); inline start Vector2 v0 = _edgeA.V0; Vector2 v1 = _edgeA.V1; Vector2 v2 = _edgeA.V2; Vector2 v3 = _edgeA.V3; // Determine allowable the normal regions based on adjacency. // Note: it may be possible that no normal is admissable. Vector2 centerB = _proxyB.Centroid; if (_edgeA.HasVertex0) { Vector2 e0 = v1 - v0; Vector2 e1 = v2 - v1; Vector2 n0 = new Vector2(e0.Y, -e0.X); Vector2 n1 = new Vector2(e1.Y, -e1.X); n0.Normalize(); n1.Normalize(); bool convex = MathUtils.Cross(n0, n1) >= 0.0f; bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f; bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f; if (convex) { if (front0 || front1) { _limit11 = n1; _limit12 = n0; } else { _limit11 = -n1; _limit12 = -n0; } } else { if (front0 && front1) { _limit11 = n0; _limit12 = n1; } else { _limit11 = -n0; _limit12 = -n1; } } } else { _limit11 = Vector2.Zero; _limit12 = Vector2.Zero; } if (_edgeA.HasVertex3) { Vector2 e1 = v2 - v1; Vector2 e2 = v3 - v2; Vector2 n1 = new Vector2(e1.Y, -e1.X); Vector2 n2 = new Vector2(e2.Y, -e2.X); n1.Normalize(); n2.Normalize(); bool convex = MathUtils.Cross(n1, n2) >= 0.0f; bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f; bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f; if (convex) { if (front1 || front2) { _limit21 = n2; _limit22 = n1; } else { _limit21 = -n2; _limit22 = -n1; } } else { if (front1 && front2) { _limit21 = n1; _limit22 = n2; } else { _limit21 = -n1; _limit22 = -n2; } } } else { _limit21 = Vector2.Zero; _limit22 = Vector2.Zero; } //ComputeAdjacency(); inline end //EPAxis edgeAxis = ComputeEdgeSeparation(); inline start EPAxis edgeAxis = ComputeEdgeSeparation(); // If no valid normal can be found than this edge should not collide. // This can happen on the middle edge of a 3-edge zig-zag chain. if (edgeAxis.Type == EPAxisType.Unknown) { return; } if (edgeAxis.Separation > _radius) { return; } EPAxis polygonAxis = ComputePolygonSeparation(); if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius) { return; } // Use hysteresis for jitter reduction. const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; EPAxis primaryAxis; if (polygonAxis.Type == EPAxisType.Unknown) { primaryAxis = edgeAxis; } else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } EPProxy proxy1; EPProxy proxy2; FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>(); if (primaryAxis.Type == EPAxisType.EdgeA) { proxy1 = _proxyA; proxy2 = _proxyB; manifold.Type = ManifoldType.FaceA; } else { proxy1 = _proxyB; proxy2 = _proxyA; manifold.Type = ManifoldType.FaceB; } int edge1 = primaryAxis.Index; FindIncidentEdge(ref incidentEdge, proxy1, primaryAxis.Index, proxy2); int count1 = proxy1.Count; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; Vector2 v11 = proxy1.Vertices[iv1]; Vector2 v12 = proxy1.Vertices[iv2]; Vector2 tangent = v12 - v11; tangent.Normalize(); Vector2 normal = MathUtils.Cross(tangent, 1.0f); Vector2 planePoint = 0.5f * (v11 + v12); // Face offset. float frontOffset = Vector2.Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vector2.Dot(tangent, v11) + _radius; float sideOffset2 = Vector2.Dot(tangent, v12) + _radius; // Clip incident edge against extruded edge1 side edges. FixedArray2<ClipVertex> clipPoints1; FixedArray2<ClipVertex> clipPoints2; int np; // Clip to box side 1 np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); if (np < Settings.MaxManifoldPoints) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); if (np < Settings.MaxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.Type == EPAxisType.EdgeA) { manifold.LocalNormal = normal; manifold.LocalPoint = planePoint; } else { manifold.LocalNormal = MathUtils.MultiplyT(ref _xf.R, ref normal); manifold.LocalPoint = MathUtils.MultiplyT(ref _xf, ref planePoint); } int pointCount = 0; for (int i1 = 0; i1 < Settings.MaxManifoldPoints; ++i1) { float separation = Vector2.Dot(normal, clipPoints2[i1].V) - frontOffset; if (separation <= _radius) { ManifoldPoint cp = manifold.Points[pointCount]; if (primaryAxis.Type == EPAxisType.EdgeA) { cp.LocalPoint = MathUtils.MultiplyT(ref _xf, clipPoints2[i1].V); cp.Id = clipPoints2[i1].ID; } else { cp.LocalPoint = clipPoints2[i1].V; cp.Id.Features.TypeA = clipPoints2[i1].ID.Features.TypeB; cp.Id.Features.TypeB = clipPoints2[i1].ID.Features.TypeA; cp.Id.Features.IndexA = clipPoints2[i1].ID.Features.IndexB; cp.Id.Features.IndexB = clipPoints2[i1].ID.Features.IndexA; } manifold.Points[pointCount] = cp; ++pointCount; } } manifold.PointCount = pointCount; //Collide(ref manifold); inline end }
/// Compute the collision manifold between two circles. public static void CollideCircles(ref Manifold manifold, CircleShape circleA, ref Transform xfA, CircleShape circleB, ref Transform xfB) { manifold.PointCount = 0; float pAx = xfA.Position.X + xfA.R.Col1.X * circleA.Position.X + xfA.R.Col2.X * circleA.Position.Y; float pAy = xfA.Position.Y + xfA.R.Col1.Y * circleA.Position.X + xfA.R.Col2.Y * circleA.Position.Y; float pBx = xfB.Position.X + xfB.R.Col1.X * circleB.Position.X + xfB.R.Col2.X * circleB.Position.Y; float pBy = xfB.Position.Y + xfB.R.Col1.Y * circleB.Position.X + xfB.R.Col2.Y * circleB.Position.Y; float distSqr = (pBx - pAx) * (pBx - pAx) + (pBy - pAy) * (pBy - pAy); float radius = circleA.Radius + circleB.Radius; if (distSqr > radius * radius) { return; } manifold.Type = ManifoldType.Circles; manifold.LocalPoint = circleA.Position; manifold.LocalNormal = Vector2.Zero; manifold.PointCount = 1; ManifoldPoint p0 = manifold.Points[0]; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; manifold.Points[0] = p0; }
/// <summary> /// Evaluate the manifold with supplied transforms. This assumes /// modest motion from the original state. This does not change the /// point count, impulses, etc. The radii must come from the Shapes /// that generated the manifold. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="transformA">The transform for A.</param> /// <param name="radiusA">The radius for A.</param> /// <param name="transformB">The transform for B.</param> /// <param name="radiusB">The radius for B.</param> /// <param name="normal">World vector pointing from A to B</param> /// <param name="points">Torld contact point (point of intersection).</param> public static void GetWorldManifold(ref Manifold manifold, ref Transform transformA, float radiusA, ref Transform transformB, float radiusB, out Vector2 normal, out FixedArray2<Vector2> points) { points = new FixedArray2<Vector2>(); normal = Vector2.Zero; if (manifold.PointCount == 0) { normal = Vector2.UnitY; return; } switch (manifold.Type) { case ManifoldType.Circles: { Vector2 tmp = manifold.Points[0].LocalPoint; float pointAx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X + transformA.R.Col2.X * manifold.LocalPoint.Y; float pointAy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X + transformA.R.Col2.Y * manifold.LocalPoint.Y; float pointBx = transformB.Position.X + transformB.R.Col1.X * tmp.X + transformB.R.Col2.X * tmp.Y; float pointBy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X + transformB.R.Col2.Y * tmp.Y; normal.X = 1; normal.Y = 0; float result = (pointAx - pointBx) * (pointAx - pointBx) + (pointAy - pointBy) * (pointAy - pointBy); if (result > Settings.Epsilon * Settings.Epsilon) { float tmpNormalx = pointBx - pointAx; float tmpNormaly = pointBy - pointAy; float factor = 1f / (float)Math.Sqrt(tmpNormalx * tmpNormalx + tmpNormaly * tmpNormaly); normal.X = tmpNormalx * factor; normal.Y = tmpNormaly * factor; } Vector2 c = Vector2.Zero; c.X = (pointAx + radiusA * normal.X) + (pointBx - radiusB * normal.X); c.Y = (pointAy + radiusA * normal.Y) + (pointBy - radiusB * normal.Y); points[0] = 0.5f * c; } break; case ManifoldType.FaceA: { normal.X = transformA.R.Col1.X * manifold.LocalNormal.X + transformA.R.Col2.X * manifold.LocalNormal.Y; normal.Y = transformA.R.Col1.Y * manifold.LocalNormal.X + transformA.R.Col2.Y * manifold.LocalNormal.Y; float planePointx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X + transformA.R.Col2.X * manifold.LocalPoint.Y; float planePointy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X + transformA.R.Col2.Y * manifold.LocalPoint.Y; for (int i = 0; i < manifold.PointCount; ++i) { Vector2 tmp = manifold.Points[i].LocalPoint; float clipPointx = transformB.Position.X + transformB.R.Col1.X * tmp.X + transformB.R.Col2.X * tmp.Y; float clipPointy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X + transformB.R.Col2.Y * tmp.Y; float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y; Vector2 c = Vector2.Zero; c.X = (clipPointx + (radiusA - value) * normal.X) + (clipPointx - radiusB * normal.X); c.Y = (clipPointy + (radiusA - value) * normal.Y) + (clipPointy - radiusB * normal.Y); points[i] = 0.5f * c; } } break; case ManifoldType.FaceB: { normal.X = transformB.R.Col1.X * manifold.LocalNormal.X + transformB.R.Col2.X * manifold.LocalNormal.Y; normal.Y = transformB.R.Col1.Y * manifold.LocalNormal.X + transformB.R.Col2.Y * manifold.LocalNormal.Y; float planePointx = transformB.Position.X + transformB.R.Col1.X * manifold.LocalPoint.X + transformB.R.Col2.X * manifold.LocalPoint.Y; float planePointy = transformB.Position.Y + transformB.R.Col1.Y * manifold.LocalPoint.X + transformB.R.Col2.Y * manifold.LocalPoint.Y; for (int i = 0; i < manifold.PointCount; ++i) { Vector2 tmp = manifold.Points[i].LocalPoint; float clipPointx = transformA.Position.X + transformA.R.Col1.X * tmp.X + transformA.R.Col2.X * tmp.Y; float clipPointy = transformA.Position.Y + transformA.R.Col1.Y * tmp.X + transformA.R.Col2.Y * tmp.Y; float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y; Vector2 c = Vector2.Zero; c.X = (clipPointx - radiusA * normal.X) + (clipPointx + (radiusB - value) * normal.X); c.Y = (clipPointy - radiusA * normal.Y) + (clipPointy + (radiusB - value) * normal.Y); points[i] = 0.5f * c; } // Ensure normal points from A to B. normal *= -1; } break; default: normal = Vector2.UnitY; break; } }
public static void GetPointStates(out FixedArray2<PointState> state1, out FixedArray2<PointState> state2, ref Manifold manifold1, ref Manifold manifold2) { state1 = new FixedArray2<PointState>(); state2 = new FixedArray2<PointState>(); // Detect persists and removes. for (int i = 0; i < manifold1.PointCount; ++i) { ContactID id = manifold1.Points[i].Id; state1[i] = PointState.Remove; for (int j = 0; j < manifold2.PointCount; ++j) { if (manifold2.Points[j].Id.Key == id.Key) { state1[i] = PointState.Persist; break; } } } // Detect persists and adds. for (int i = 0; i < manifold2.PointCount; ++i) { ContactID id = manifold2.Points[i].Id; state2[i] = PointState.Add; for (int j = 0; j < manifold1.PointCount; ++j) { if (manifold1.Points[j].Id.Key == id.Key) { state2[i] = PointState.Persist; break; } } } }
/// <summary> /// Compute the collision manifold between two polygons. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="polyA">The poly A.</param> /// <param name="transformA">The transform A.</param> /// <param name="polyB">The poly B.</param> /// <param name="transformB">The transform B.</param> public static void CollidePolygons(ref Manifold manifold, PolygonShape polyA, ref Transform transformA, PolygonShape polyB, ref Transform transformB) { manifold.PointCount = 0; float totalRadius = polyA.Radius + polyB.Radius; int edgeA = 0; float separationA = FindMaxSeparation(out edgeA, polyA, ref transformA, polyB, ref transformB); if (separationA > totalRadius) return; int edgeB = 0; float separationB = FindMaxSeparation(out edgeB, polyB, ref transformB, polyA, ref transformA); if (separationB > totalRadius) return; PolygonShape poly1; // reference polygon PolygonShape poly2; // incident polygon Transform xf1, xf2; int edge1; // reference edge bool 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 = transformB; xf2 = transformA; edge1 = edgeB; manifold.Type = ManifoldType.FaceB; flip = true; } else { poly1 = polyA; poly2 = polyB; xf1 = transformA; xf2 = transformB; edge1 = edgeA; manifold.Type = ManifoldType.FaceA; flip = false; } FixedArray2<ClipVertex> incidentEdge; FindIncidentEdge(out incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2); int count1 = poly1.Vertices.Count; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; Vector2 v11 = poly1.Vertices[iv1]; Vector2 v12 = poly1.Vertices[iv2]; float localTangentX = v12.X - v11.X; float localTangentY = v12.Y - v11.Y; float factor = 1f / (float)Math.Sqrt(localTangentX * localTangentX + localTangentY * localTangentY); localTangentX = localTangentX * factor; localTangentY = localTangentY * factor; Vector2 localNormal = new Vector2(localTangentY, -localTangentX); Vector2 planePoint = 0.5f * (v11 + v12); Vector2 tangent = new Vector2(xf1.R.Col1.X * localTangentX + xf1.R.Col2.X * localTangentY, xf1.R.Col1.Y * localTangentX + xf1.R.Col2.Y * localTangentY); float normalx = tangent.Y; float normaly = -tangent.X; v11 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v11.X + xf1.R.Col2.X * v11.Y, xf1.Position.Y + xf1.R.Col1.Y * v11.X + xf1.R.Col2.Y * v11.Y); v12 = new Vector2(xf1.Position.X + xf1.R.Col1.X * v12.X + xf1.R.Col2.X * v12.Y, xf1.Position.Y + xf1.R.Col1.Y * v12.X + xf1.R.Col2.Y * v12.Y); // Face offset. float frontOffset = normalx * v11.X + normaly * v11.Y; // Side offsets, extended by polytope skin thickness. float sideOffset1 = -(tangent.X * v11.X + tangent.Y * v11.Y) + totalRadius; float sideOffset2 = tangent.X * v12.X + tangent.Y * v12.Y + totalRadius; // Clip incident edge against extruded edge1 side edges. FixedArray2<ClipVertex> clipPoints1; FixedArray2<ClipVertex> clipPoints2; // Clip to box side 1 int np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); if (np < 2) return; // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, 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 < Settings.MaxManifoldPoints; ++i) { Vector2 value = clipPoints2[i].V; float separation = normalx * value.X + normaly * value.Y - frontOffset; if (separation <= totalRadius) { ManifoldPoint cp = manifold.Points[pointCount]; Vector2 tmp = clipPoints2[i].V; float tmp1X = tmp.X - xf2.Position.X; float tmp1Y = tmp.Y - xf2.Position.Y; cp.LocalPoint.X = tmp1X * xf2.R.Col1.X + tmp1Y * xf2.R.Col1.Y; cp.LocalPoint.Y = tmp1X * xf2.R.Col2.X + tmp1Y * xf2.R.Col2.Y; cp.Id = clipPoints2[i].ID; if (flip) { // Swap features ContactFeature cf = cp.Id.Features; cp.Id.Features.IndexA = cf.IndexB; cp.Id.Features.IndexB = cf.IndexA; cp.Id.Features.TypeA = cf.TypeB; cp.Id.Features.TypeB = cf.TypeA; } manifold.Points[pointCount] = cp; ++pointCount; } } manifold.PointCount = pointCount; }
/// <summary> /// Compute the collision manifold between a polygon and a circle. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="polygonA">The polygon A.</param> /// <param name="transformA">The transform of A.</param> /// <param name="circleB">The circle B.</param> /// <param name="transformB">The transform of B.</param> public static void CollidePolygonAndCircle(ref Manifold manifold, PolygonShape polygonA, ref Transform transformA, CircleShape circleB, ref Transform transformB) { manifold.PointCount = 0; // Compute circle position in the frame of the polygon. Vector2 c = new Vector2( transformB.Position.X + transformB.R.Col1.X * circleB.Position.X + transformB.R.Col2.X * circleB.Position.Y, transformB.Position.Y + transformB.R.Col1.Y * circleB.Position.X + transformB.R.Col2.Y * circleB.Position.Y); Vector2 cLocal = new Vector2( (c.X - transformA.Position.X) * transformA.R.Col1.X + (c.Y - transformA.Position.Y) * transformA.R.Col1.Y, (c.X - transformA.Position.X) * transformA.R.Col2.X + (c.Y - transformA.Position.Y) * transformA.R.Col2.Y); // Find the min separating edge. int normalIndex = 0; float separation = -Settings.MaxFloat; float radius = polygonA.Radius + circleB.Radius; int vertexCount = polygonA.Vertices.Count; for (int i = 0; i < vertexCount; ++i) { Vector2 value1 = polygonA.Normals[i]; Vector2 value2 = cLocal - polygonA.Vertices[i]; float s = value1.X * value2.X + value1.Y * value2.Y; 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; Vector2 v1 = polygonA.Vertices[vertIndex1]; Vector2 v2 = polygonA.Vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < Settings.Epsilon) { manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = polygonA.Normals[normalIndex]; manifold.LocalPoint = 0.5f * (v1 + v2); ManifoldPoint p0 = manifold.Points[0]; p0.LocalPoint = circleB.Position; p0.Id.Key = 0; manifold.Points[0] = p0; return; } // Compute barycentric coordinates float u1 = (cLocal.X - v1.X) * (v2.X - v1.X) + (cLocal.Y - v1.Y) * (v2.Y - v1.Y); float u2 = (cLocal.X - v2.X) * (v1.X - v2.X) + (cLocal.Y - v2.Y) * (v1.Y - v2.Y); if (u1 <= 0.0f) { float r = (cLocal.X - v1.X) * (cLocal.X - v1.X) + (cLocal.Y - v1.Y) * (cLocal.Y - v1.Y); if (r > radius * radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = cLocal - v1; float factor = 1f / (float) Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X + manifold.LocalNormal.Y * manifold.LocalNormal.Y); manifold.LocalNormal.X = manifold.LocalNormal.X * factor; manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor; manifold.LocalPoint = v1; ManifoldPoint p0b = manifold.Points[0]; p0b.LocalPoint = circleB.Position; p0b.Id.Key = 0; manifold.Points[0] = p0b; } else if (u2 <= 0.0f) { float r = (cLocal.X - v2.X) * (cLocal.X - v2.X) + (cLocal.Y - v2.Y) * (cLocal.Y - v2.Y); if (r > radius * radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = cLocal - v2; float factor = 1f / (float) Math.Sqrt(manifold.LocalNormal.X * manifold.LocalNormal.X + manifold.LocalNormal.Y * manifold.LocalNormal.Y); manifold.LocalNormal.X = manifold.LocalNormal.X * factor; manifold.LocalNormal.Y = manifold.LocalNormal.Y * factor; manifold.LocalPoint = v2; ManifoldPoint p0c = manifold.Points[0]; p0c.LocalPoint = circleB.Position; p0c.Id.Key = 0; manifold.Points[0] = p0c; } else { Vector2 faceCenter = 0.5f * (v1 + v2); Vector2 value1 = cLocal - faceCenter; Vector2 value2 = polygonA.Normals[vertIndex1]; float separation2 = value1.X * value2.X + value1.Y * value2.Y; if (separation2 > radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = polygonA.Normals[vertIndex1]; manifold.LocalPoint = faceCenter; ManifoldPoint p0d = manifold.Points[0]; p0d.LocalPoint = circleB.Position; p0d.Id.Key = 0; manifold.Points[0] = p0d; } }