/// <summary> /// 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. /// </summary> public static void GetPointStates(PointState[] /*b2_maxManifoldPoints*/ state1, PointState[] /*b2_maxManifoldPoints*/ state2, Manifold manifold1, Manifold manifold2) { for (int i = 0; i < Common.Settings.MaxManifoldPoints; ++i) { state1[i] = PointState.NullState; state2[i] = PointState.NullState; } // Detect persists and removes. for (int i = 0; i < manifold1.PointCount; ++i) { ContactID id = manifold1.Points[i].ID; state1[i] = PointState.RemoveState; for (int j = 0; j < manifold2.PointCount; ++j) { if (manifold2.Points[j].ID.Key == id.Key) { state1[i] = PointState.PersistState; break; } } } // Detect persists and adds. for (int i = 0; i < manifold2.PointCount; ++i) { ContactID id = manifold2.Points[i].ID; state2[i] = PointState.AddState; for (int j = 0; j < manifold1.PointCount; ++j) { if (manifold1.Points[j].ID.Key == id.Key) { state2[i] = PointState.PersistState; break; } } } }
// This implements 2-sided edge vs circle collision. public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edge, XForm transformA, CircleShape circle, XForm transformB) { manifold.PointCount = 0; Vec2 cLocal = Common.Math.MulT(transformA, Common.Math.Mul(transformB, circle._position)); Vec2 normal = edge._normal; Vec2 v1 = edge._v1; Vec2 v2 = edge._v2; float radius = edge._radius + circle._radius; // Barycentric coordinates float u1 = Vec2.Dot(cLocal - v1, v2 - v1); float u2 = Vec2.Dot(cLocal - v2, v1 - v2); if (u1 <= 0.0f) { // Behind v1 if (Vec2.DistanceSquared(cLocal, v1) > radius * radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalPlaneNormal = cLocal - v1; manifold.LocalPlaneNormal.Normalize(); manifold.LocalPoint = v1; manifold.Points[0].LocalPoint = circle._position; manifold.Points[0].ID.Key = 0; } else if (u2 <= 0.0f) { // Ahead of v2 if (Vec2.DistanceSquared(cLocal, v2) > radius * radius) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalPlaneNormal = cLocal - v2; manifold.LocalPlaneNormal.Normalize(); manifold.LocalPoint = v2; manifold.Points[0].LocalPoint = circle._position; manifold.Points[0].ID.Key = 0; } else { float separation = Vec2.Dot(cLocal - v1, normal); if (separation < -radius || radius < separation) { return; } manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalPlaneNormal = separation < 0.0f ? -normal : normal; manifold.LocalPoint = 0.5f * (v1 + v2); manifold.Points[0].LocalPoint = circle._position; manifold.Points[0].ID.Key = 0; } }
/// 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. public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB) { if (manifold.PointCount == 0) { return; } switch (manifold.Type) { case ManifoldType.Circles: { Vector2 pointA = xfA.TransformPoint(manifold.LocalPoint); Vector2 pointB = xfB.TransformPoint(manifold.Points[0].LocalPoint); Vector2 normal = new Vector2(1.0f, 0.0f); if ((pointA - pointB).LengthSquared() > (Box2DNet.Common.Math.Epsilon * Box2DNet.Common.Math.Epsilon)) { normal = pointB - pointA; normal.Normalize(); } Normal = normal; Vector2 cA = pointA + radiusA * normal; Vector2 cB = pointB - radiusB * normal; Points[0] = 0.5f * (cA + cB); } break; case ManifoldType.FaceA: { Vector2 normal = xfA.TransformDirection(manifold.LocalPlaneNormal); Vector2 planePoint = xfA.TransformPoint(manifold.LocalPoint); // Ensure normal points from A to B. Normal = normal; for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = xfB.TransformPoint(manifold.Points[i].LocalPoint); Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Vector2 cB = clipPoint - radiusB * normal; Points[i] = 0.5f * (cA + cB); } } break; case ManifoldType.FaceB: { Vector2 normal = xfB.TransformDirection(manifold.LocalPlaneNormal); Vector2 planePoint = xfB.TransformPoint(manifold.LocalPoint); // Ensure normal points from A to B. Normal = -normal; for (int i = 0; i < manifold.PointCount; ++i) { Vector2 clipPoint = xfA.TransformPoint(manifold.Points[i].LocalPoint); Vector2 cA = clipPoint - radiusA * normal; Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal; Points[i] = 0.5f * (cA + cB); } } break; } }