Box2D.Collision.WorldManifold.Initialize C# (CSharp) Метод

        public void Initialize(Manifold manifold, Transform xfA, float radiusA, Transform xfB, float radiusB)
        {
            if (manifold.PointCount == 0)
            {
                return;
            }

            switch (manifold.Type)
            {

                case Manifold.ManifoldType.Circles:
                    {
                        // final Vec2 pointA = pool3;
                        // final Vec2 pointB = pool4;
                        //
                        // normal.set(1, 0);
                        // Transform.mulToOut(xfA, manifold.localPoint, pointA);
                        // Transform.mulToOut(xfB, manifold.points[0].localPoint, pointB);
                        //
                        // if (MathUtils.distanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON) {
                        // normal.set(pointB).subLocal(pointA);
                        // normal.normalize();
                        // }
                        //
                        // cA.set(normal).mulLocal(radiusA).addLocal(pointA);
                        // cB.set(normal).mulLocal(radiusB).subLocal(pointB).negateLocal();
                        // points[0].set(cA).addLocal(cB).mulLocal(0.5f);
                        Vec2 pointA = pool3;
                        Vec2 pointB = pool4;

                        Normal.X = 1;
                        Normal.Y = 0;
                        // pointA.x = xfA.p.x + xfA.q.ex.x * manifold.localPoint.x + xfA.q.ey.x *
                        // manifold.localPoint.y;
                        // pointA.y = xfA.p.y + xfA.q.ex.y * manifold.localPoint.x + xfA.q.ey.y *
                        // manifold.localPoint.y;
                        // pointB.x = xfB.p.x + xfB.q.ex.x * manifold.points[0].localPoint.x + xfB.q.ey.x *
                        // manifold.points[0].localPoint.y;
                        // pointB.y = xfB.p.y + xfB.q.ex.y * manifold.points[0].localPoint.x + xfB.q.ey.y *
                        // manifold.points[0].localPoint.y;
                        Transform.MulToOut(xfA, manifold.LocalPoint, pointA);
                        Transform.MulToOut(xfB, manifold.Points[0].LocalPoint, pointB);

                        if (MathUtils.DistanceSquared(pointA, pointB) > Settings.EPSILON * Settings.EPSILON)
                        {
                            Normal.X = pointB.X - pointA.X;
                            Normal.Y = pointB.Y - pointA.Y;
                            Normal.Normalize();
                        }

                        float cAx = Normal.X * radiusA + pointA.X;
                        float cAy = Normal.Y * radiusA + pointA.Y;

                        float cBx = (-Normal.X) * radiusB + pointB.X;
                        float cBy = (-Normal.Y) * radiusB + pointB.Y;

                        Points[0].X = (cAx + cBx) * .5f;
                        Points[0].Y = (cAy + cBy) * .5f;
                    }
                    break;

                case Manifold.ManifoldType.FaceA:
                    {
                        Vec2 planePoint = pool3;

                        Rot.MulToOutUnsafe(xfA.Q, manifold.LocalNormal, Normal);
                        Transform.MulToOut(xfA, manifold.LocalPoint, planePoint);

                        Vec2 clipPoint = pool4;

                        for (int i = 0; i < manifold.PointCount; i++)
                        {
                            // b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
                            // b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint,
                            // normal)) * normal;
                            // b2Vec2 cB = clipPoint - radiusB * normal;
                            // points[i] = 0.5f * (cA + cB);
                            Transform.MulToOut(xfB, manifold.Points[i].LocalPoint, clipPoint);
                            // use cA as temporary for now
                            // cA.set(clipPoint).subLocal(planePoint);
                            // float scalar = radiusA - Vec2.dot(cA, normal);
                            // cA.set(normal).mulLocal(scalar).addLocal(clipPoint);
                            // cB.set(normal).mulLocal(radiusB).subLocal(clipPoint).negateLocal();
                            // points[i].set(cA).addLocal(cB).mulLocal(0.5f);

                            float scalar = radiusA - ((clipPoint.X - planePoint.X) * Normal.X + (clipPoint.Y - planePoint.Y) * Normal.Y);

                            float cAx = Normal.X * scalar + clipPoint.X;
                            float cAy = Normal.Y * scalar + clipPoint.Y;

                            float cBx = (-Normal.X) * radiusB + clipPoint.X;
                            float cBy = (-Normal.Y) * radiusB + clipPoint.Y;

                            Points[i].X = (cAx + cBx) * .5f;
                            Points[i].Y = (cAy + cBy) * .5f;
                        }
                    }
                    break;

                case Manifold.ManifoldType.FaceB:
                    Vec2 planePoint2 = pool3;
                    Rot.MulToOutUnsafe(xfB.Q, manifold.LocalNormal, Normal);
                    Transform.MulToOut(xfB, manifold.LocalPoint, planePoint2);

                    // final Mat22 R = xfB.q;
                    // normal.x = R.ex.x * manifold.localNormal.x + R.ey.x * manifold.localNormal.y;
                    // normal.y = R.ex.y * manifold.localNormal.x + R.ey.y * manifold.localNormal.y;
                    // final Vec2 v = manifold.localPoint;
                    // planePoint.x = xfB.p.x + xfB.q.ex.x * v.x + xfB.q.ey.x * v.y;
                    // planePoint.y = xfB.p.y + xfB.q.ex.y * v.x + xfB.q.ey.y * v.y;

                    Vec2 clipPoint2 = pool4;

                    for (int i = 0; i < manifold.PointCount; i++)
                    {
                        // b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
                        // b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint,
                        // normal)) * normal;
                        // b2Vec2 cA = clipPoint - radiusA * normal;
                        // points[i] = 0.5f * (cA + cB);

                        Transform.MulToOut(xfA, manifold.Points[i].LocalPoint, clipPoint2);
                        // cB.set(clipPoint).subLocal(planePoint);
                        // float scalar = radiusB - Vec2.dot(cB, normal);
                        // cB.set(normal).mulLocal(scalar).addLocal(clipPoint);
                        // cA.set(normal).mulLocal(radiusA).subLocal(clipPoint).negateLocal();
                        // points[i].set(cA).addLocal(cB).mulLocal(0.5f);

                        // points[i] = 0.5f * (cA + cB);

                        //
                        // clipPoint.x = xfA.p.x + xfA.q.ex.x * manifold.points[i].localPoint.x + xfA.q.ey.x *
                        // manifold.points[i].localPoint.y;
                        // clipPoint.y = xfA.p.y + xfA.q.ex.y * manifold.points[i].localPoint.x + xfA.q.ey.y *
                        // manifold.points[i].localPoint.y;

                        float scalar = radiusB - ((clipPoint2.X - planePoint2.X) * Normal.X + (clipPoint2.Y - planePoint2.Y) * Normal.Y);

                        float cBx = Normal.X * scalar + clipPoint2.X;
                        float cBy = Normal.Y * scalar + clipPoint2.Y;

                        float cAx = (-Normal.X) * radiusA + clipPoint2.X;
                        float cAy = (-Normal.Y) * radiusA + clipPoint2.Y;

                        Points[i].X = (cAx + cBx) * .5f;
                        Points[i].Y = (cAy + cBy) * .5f;
                    }
                    // Ensure normal points from A to B.
                    Normal.X = -Normal.X;
                    Normal.Y = -Normal.Y;
                    break;
            }
        }

        /// <summary>
        /// Get the world manifold.
        /// </summary>
        public void GetWorldManifold(WorldManifold worldManifold)
        {
            Body bodyA = FixtureA.Body;
            Body bodyB = FixtureB.Body;
            Shape shapeA = FixtureA.Shape;
            Shape shapeB = FixtureB.Shape;

            worldManifold.Initialize(Manifold, bodyA.GetTransform(), shapeA.Radius, bodyB.GetTransform(), shapeB.Radius);
        }