Box2DX.Collision.Simplex.Solve2 C# (CSharp) Метод

		internal void Solve2()
		{
			Vector2 w1 = _v1.w;
			Vector2 w2 = _v2.w;
			Vector2 e12 = w2 - w1;

			// w1 region
			float d12_2 = -Vector2.Dot(w1, e12);
			if (d12_2 <= 0.0f)
			{
				// a2 <= 0, so we clamp it to 0
				_v1.a = 1.0f;
				_count = 1;
				return;
			}

			// w2 region
			float d12_1 = Vector2.Dot(w2, e12);
			if (d12_1 <= 0.0f)
			{
				// a1 <= 0, so we clamp it to 0
				_v2.a = 1.0f;
				_count = 1;
				_v1 = _v2;
				return;
			}

			// Must be in e12 region.
			float inv_d12 = 1.0f / (d12_1 + d12_2);
			_v1.a = d12_1 * inv_d12;
			_v2.a = d12_2 * inv_d12;
			_count = 2;
		}

        /// <summary>
        /// Compute the closest points between two shapes. Supports any combination of:
        /// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.
        /// On the first call set b2SimplexCache.count to zero.
        /// </summary>
        public static void Distance(out DistanceOutput output,
                        SimplexCache cache,
                        DistanceInput input)
        {
            ++GjkCalls;

            DistanceProxy proxyA = input.proxyA;
            DistanceProxy proxyB = input.proxyB;

            Transform transformA = input.TransformA;
            Transform transformB = input.TransformB;

            // Initialize the simplex.
            Simplex simplex = new Simplex();
            simplex.ReadCache(cache, proxyA, ref  transformA, proxyB, ref transformB);

            // Get simplex vertices as an array.
            SimplexVertex[] vertices = simplex.Vertices;
            const int k_maxIters = 20;

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
            int[] saveA = new int[3], saveB = new int[3];
            int saveCount = 0;

            Vec2 closestPoint = simplex.GetClosestPoint();
            float distanceSqr1 = closestPoint.LengthSquared();
            float distanceSqr2 = distanceSqr1;

            // Main iteration loop.
            int iter = 0;
            while (iter < k_maxIters)
            {
                // Copy simplex so we can identify duplicates.
                saveCount = simplex.Count;
                for (int i = 0; i < saveCount; ++i)
                {
                    saveA[i] = vertices[i].IndexA;
                    saveB[i] = vertices[i].IndexB;
                }

                switch (simplex.Count)
                {
                    case 1:
                        break;

                    case 2:
                        simplex.Solve2();
                        break;

                    case 3:
                        simplex.Solve3();
                        break;

                    default:
                        Box2DXDebug.Assert(false);
                        break;
                }

                // If we have 3 points, then the origin is in the corresponding triangle.
                if (simplex.Count == 3)
                {
                    break;
                }

                // Compute closest point.
                Vec2 p = simplex.GetClosestPoint();
                float distanceSqr = p.LengthSquared();

                // Ensure progress
                if (distanceSqr2 >= distanceSqr1)
                {
                    //break;
                }
                distanceSqr1 = distanceSqr2;

                // Get search direction.
                Vec2 d = simplex.GetSearchDirection();

                // Ensure the search direction is numerically fit.
                if (d.LengthSquared() < Settings.FLT_EPSILON * Settings.FLT_EPSILON)
                {
                    // The origin is probably contained by a line segment
                    // or triangle. Thus the shapes are overlapped.

                    // We can't return zero here even though there may be overlap.
                    // In case the simplex is a point, segment, or triangle it is difficult
                    // to determine if the origin is contained in the CSO or very close to it.
                    break;
                }
                // Compute a tentative new simplex vertex using support points.
                SimplexVertex vertex = vertices[simplex.Count];
                vertex.IndexA = proxyA.GetSupport(Math.MulT(transformA.R, -d));
                vertex.WA = Math.Mul(transformA, proxyA.GetVertex(vertex.IndexA));

                vertex.IndexB = proxyB.GetSupport(Math.MulT(transformB.R, d));
                vertex.WB = Math.Mul(transformB, proxyB.GetVertex(vertex.IndexB));
                vertex.W = vertex.WB - vertex.WA;

                // Iteration count is equated to the number of support point calls.
                ++iter;
                ++GjkIters;

                // Check for duplicate support points. This is the main termination criteria.
                bool duplicate = false;
                for (int i = 0; i < saveCount; ++i)
                {
                    if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i])
                    {
                        duplicate = true;
                        break;
                    }
                }

                // If we found a duplicate support point we must exit to avoid cycling.
                if (duplicate)
                {
                    break;
                }

                // New vertex is ok and needed.
                ++simplex.Count;
            }

            GjkMaxIters = Math.Max(GjkMaxIters, iter);

            // Prepare output.
            simplex.GetWitnessPoints(out output.PointA, out output.PointB);
            output.Distance = Vec2.Distance(output.PointA, output.PointB);
            output.Iterations = iter;

            // Cache the simplex.
            simplex.WriteCache(cache);

            // Apply radii if requested.
            if (input.UseRadii)
            {
                float rA = proxyA._radius;
                float rB = proxyB._radius;

                if (output.Distance > rA + rB && output.Distance > Settings.FLT_EPSILON)
                {
                    // Shapes are still no overlapped.
                    // Move the witness points to the outer surface.
                    output.Distance -= rA + rB;
                    Vec2 normal = output.PointB - output.PointA;
                    normal.Normalize();
                    output.PointA += rA * normal;
                    output.PointB -= rB * normal;
                }
                else
                {
                    // Shapes are overlapped when radii are considered.
                    // Move the witness points to the middle.
                    Vec2 p = 0.5f * (output.PointA + output.PointB);
                    output.PointA = p;
                    output.PointB = p;
                    output.Distance = 0.0f;
                }
            }
        }