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

		static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
		{
			output = new DistanceOutput();

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

			// Initialize the simplex.
			Simplex simplex = new Simplex();
#if ALLOWUNSAFE
			fixed (SimplexCache* sPtr = &cache)
			{
				simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
			}
#else
			simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif

			// Get simplex vertices as an array.
#if ALLOWUNSAFE
			SimplexVertex* vertices = &simplex._v1;
#else
			SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif 

			// These store the vertices of the last simplex so that we
			// can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
			int* lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
			int[] lastA = new int[4];
			int[] lastB = new int[4];
#endif // ALLOWUNSAFE
			int lastCount;

			// Main iteration loop.
			int iter = 0;
			const int k_maxIterationCount = 20;
			while (iter < k_maxIterationCount)
			{
				// Copy simplex so we can identify duplicates.
				lastCount = simplex._count;
				int i;
				for (i = 0; i < lastCount; ++i)
				{
					lastA[i] = vertices[i].indexA;
					lastB[i] = vertices[i].indexB;
				}

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

					case 2:
						simplex.Solve2();
						break;

					case 3:
						simplex.Solve3();
						break;

					default:
#if DEBUG
						Box2DXDebug.Assert(false);
#endif
						break;
				}

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

				// Compute closest point.
				Vector2 p = simplex.GetClosestPoint();
				float distanceSqr = p.sqrMagnitude;

				// Ensure the search direction is numerically fit.
				if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
				{
					// 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.
#if ALLOWUNSAFE
				SimplexVertex* vertex = vertices + simplex._count;
				vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
				vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
				//Vec2 wBLocal;
				vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
				vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
				vertex->w = vertex->wB - vertex->wA;
#else
				SimplexVertex vertex = vertices[simplex._count - 1];
				vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
				vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
				//Vec2 wBLocal;
				vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
				vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
				vertex.w = vertex.wB - vertex.wA;	
#endif // ALLOWUNSAFE

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

				// Check for convergence.
#if ALLOWUNSAFE
				float lowerBound = Vector2.Dot(p, vertex->w);
#else
				float lowerBound = Vector2.Dot(p, vertex.w);
#endif
				float upperBound = distanceSqr;
				const float k_relativeTolSqr = 0.01f * 0.01f;	// 1:100
				if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
				{
					// Converged!
					break;
				}

				// Check for duplicate support points.
				bool duplicate = false;
				for (i = 0; i < lastCount; ++i)
				{
#if ALLOWUNSAFE
					if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
					if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
					{
						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;
			}

			
#if ALLOWUNSAFE
			fixed (DistanceOutput* doPtr = &output)
			{
				// Prepare output.
				simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
				doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB);
				doPtr->Iterations = iter;
			}

			fixed (SimplexCache* sPtr = &cache)
			{
				// Cache the simplex.
				simplex.WriteCache(sPtr);
			}
#else
			// Prepare output.
			simplex.GetWitnessPoints(out output.PointA, out output.PointB);
			output.Distance = Vector2.Distance(output.PointA, output.PointB);
			output.Iterations = iter;
			
			// Cache the simplex.
			simplex.WriteCache(cache);
#endif

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

				if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_EPSILON)
				{
					// Shapes are still no overlapped.
					// Move the witness points to the outer surface.
					output.Distance -= rA + rB;
					Vector2 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.
					Vector2 p = 0.5f * (output.PointA + output.PointB);
					output.PointA = p;
					output.PointB = p;
					output.Distance = 0.0f;
				}
			}
		}
	}

        // This algorithm uses conservative advancement to compute the time of
        // impact (TOI) of two shapes.
        // Refs: Bullet, Young Kim
        /// <summary>
        /// Compute the time when two shapes begin to touch or touch at a closer distance.
        /// warning the sweeps must have the same time interval.
        /// </summary>
        /// <param name="shape1"></param>
        /// <param name="sweep1"></param>
        /// <param name="shape2"></param>
        /// <param name="sweep2"></param>
        /// <returns>
        /// The fraction between [0,1] in which the shapes first touch.
        /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
        /// </returns>
#warning : "check params"
        public static float TimeOfImpact(Shape shape1, Sweep sweep1, Shape shape2, Sweep sweep2)
        {
            float r1 = shape1.GetSweepRadius();
            float r2 = shape2.GetSweepRadius();

            Box2DXDebug.Assert(sweep1.T0 == sweep2.T0);
            Box2DXDebug.Assert(1.0f - sweep1.T0 > Common.Settings.FLT_EPSILON);

            float t0     = sweep1.T0;
            Vec2  v1     = sweep1.C - sweep1.C0;
            Vec2  v2     = sweep2.C - sweep2.C0;
            float omega1 = sweep1.A - sweep1.A0;
            float omega2 = sweep2.A - sweep2.A0;

            float alpha = 0.0f;

            Vec2  p1, p2;
            int   k_maxIterations = 20;                 // TODO_ERIN b2Settings
            int   iter            = 0;
            Vec2  normal          = Vec2.Zero;
            float distance        = 0.0f;
            float targetDistance  = 0.0f;

            for (; ;)
            {
                float t = (1.0f - alpha) * t0 + alpha;
                XForm xf1, xf2;
                sweep1.GetXForm(out xf1, t);
                sweep2.GetXForm(out xf2, t);

                // Get the distance between shapes.
                distance = Collision.Distance(out p1, out p2, shape1, xf1, shape2, xf2);

                if (iter == 0)
                {
                    // Compute a reasonable target distance to give some breathing room
                    // for conservative advancement.
                    if (distance > 2.0f * Settings.ToiSlop)
                    {
                        targetDistance = 1.5f * Settings.ToiSlop;
                    }
                    else
                    {
                        targetDistance = Common.Math.Max(0.05f * Settings.ToiSlop, distance - 0.5f * Settings.ToiSlop);
                    }
                }

                if (distance - targetDistance < 0.05f * Settings.ToiSlop || iter == k_maxIterations)
                {
                    break;
                }

                normal = p2 - p1;
                normal.Normalize();

                // Compute upper bound on remaining movement.
                float approachVelocityBound = Vec2.Dot(normal, v1 - v2) +
                                              Common.Math.Abs(omega1) * r1 + Common.Math.Abs(omega2) * r2;
                if (Common.Math.Abs(approachVelocityBound) < Common.Settings.FLT_EPSILON)
                {
                    alpha = 1.0f;
                    break;
                }

                // Get the conservative time increment. Don't advance all the way.
                float dAlpha = (distance - targetDistance) / approachVelocityBound;
                //float dt = (distance - 0.5f * Settings.LinearSlop) / approachVelocityBound;
                float newAlpha = alpha + dAlpha;

                // The shapes may be moving apart or a safe distance apart.
                if (newAlpha < 0.0f || 1.0f < newAlpha)
                {
                    alpha = 1.0f;
                    break;
                }

                // Ensure significant advancement.
                if (newAlpha < (1.0f + 100.0f * Common.Settings.FLT_EPSILON) * alpha)
                {
                    break;
                }

                alpha = newAlpha;

                ++iter;
            }

            return(alpha);
        }