/// <summary> /// Gets the world manifold. /// </summary> public void GetWorldManifold(out TSVector2 normal, out FixedArray2 <TSVector2> points) { Body bodyA = FixtureA.Body; Body bodyB = FixtureB.Body; Shape shapeA = FixtureA.Shape; Shape shapeB = FixtureB.Shape; ContactSolver.WorldManifold.Initialize(ref Manifold, ref bodyA._xf, shapeA.Radius, ref bodyB._xf, shapeB.Radius, out normal, out points); }
public void GetWorldManifold(out TSVector2 normal, out FixedArray2 <TSVector2> points) { Body body = this.FixtureA.Body; Body body2 = this.FixtureB.Body; Shape shape = this.FixtureA.Shape; Shape shape2 = this.FixtureB.Shape; ContactSolver.WorldManifold.Initialize(ref this.Manifold, ref body._xf, shape.Radius, ref body2._xf, shape2.Radius, out normal, out points); }
public static void Initialize(ref Manifold manifold, ref Transform xfA, FP radiusA, ref Transform xfB, FP radiusB, out TSVector2 normal, out FixedArray2 <TSVector2> points) { normal = TSVector2.zero; points = default(FixedArray2 <TSVector2>); bool flag = manifold.PointCount == 0; if (!flag) { switch (manifold.Type) { case ManifoldType.Circles: { normal = new TSVector2(1f, 0f); TSVector2 tSVector = MathUtils.Mul(ref xfA, manifold.LocalPoint); TSVector2 tSVector2 = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint); bool flag2 = TSVector2.DistanceSquared(tSVector, tSVector2) > Settings.EpsilonSqr; if (flag2) { normal = tSVector2 - tSVector; normal.Normalize(); } TSVector2 value = tSVector + radiusA * normal; TSVector2 value2 = tSVector2 - radiusB * normal; points[0] = 0.5f * (value + value2); break; } case ManifoldType.FaceA: { normal = MathUtils.Mul(xfA.q, manifold.LocalNormal); TSVector2 value3 = MathUtils.Mul(ref xfA, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; i++) { TSVector2 value4 = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint); TSVector2 value5 = value4 + (radiusA - TSVector2.Dot(value4 - value3, normal)) * normal; TSVector2 value6 = value4 - radiusB * normal; points[i] = 0.5f * (value5 + value6); } break; } case ManifoldType.FaceB: { normal = MathUtils.Mul(xfB.q, manifold.LocalNormal); TSVector2 value7 = MathUtils.Mul(ref xfB, manifold.LocalPoint); for (int j = 0; j < manifold.PointCount; j++) { TSVector2 value8 = MathUtils.Mul(ref xfA, manifold.Points[j].LocalPoint); TSVector2 value9 = value8 + (radiusB - TSVector2.Dot(value8 - value7, normal)) * normal; TSVector2 value10 = value8 - radiusA * normal; points[j] = 0.5f * (value10 + value9); } normal = -normal; break; } } } }
/// <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="xfA">The transform for A.</param> /// <param name="radiusA">The radius for A.</param> /// <param name="xfB">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 Initialize(ref Manifold manifold, ref Transform xfA, FP radiusA, ref Transform xfB, FP radiusB, out TSVector2 normal, out FixedArray2 <TSVector2> points) { normal = TSVector2.zero; points = new FixedArray2 <TSVector2>(); if (manifold.PointCount == 0) { return; } switch (manifold.Type) { case ManifoldType.Circles: { normal = new TSVector2(1.0f, 0.0f); TSVector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint); TSVector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint); if (TSVector2.DistanceSquared(pointA, pointB) > Settings.EpsilonSqr) { normal = pointB - pointA; normal.Normalize(); } TSVector2 cA = pointA + radiusA * normal; TSVector2 cB = pointB - radiusB * normal; points[0] = 0.5f * (cA + cB); } break; case ManifoldType.FaceA: { normal = MathUtils.Mul(xfA.q, manifold.LocalNormal); TSVector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; ++i) { TSVector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint); TSVector2 cA = clipPoint + (radiusA - TSVector2.Dot(clipPoint - planePoint, normal)) * normal; TSVector2 cB = clipPoint - radiusB * normal; points[i] = 0.5f * (cA + cB); } } break; case ManifoldType.FaceB: { normal = MathUtils.Mul(xfB.q, manifold.LocalNormal); TSVector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint); for (int i = 0; i < manifold.PointCount; ++i) { TSVector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint); TSVector2 cB = clipPoint + (radiusB - TSVector2.Dot(clipPoint - planePoint, normal)) * normal; TSVector2 cA = clipPoint - radiusA * normal; points[i] = 0.5f * (cA + cB); } // Ensure normal points from A to B. normal = -normal; } break; } }