Ejemplo n.º 1
0
        /// <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);
        }
Ejemplo n.º 2
0
        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);
        }
Ejemplo n.º 3
0
            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;
                }
            }