示例#1
0
        public static void Distance(b2DistanceOutput output, b2SimplexCache cache, b2DistanceInput input)
        {
            ++b2_gjkCalls;

            b2DistanceProxy proxyA = input.proxyA;
            b2DistanceProxy proxyB = input.proxyB;

            b2Transform transformA = input.transformA;
            b2Transform transformB = input.transformB;

            // Initialize the simplex
            b2Simplex simplex = s_simplex;

            simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);

            // Get simplex vertices as an vector.
            b2SimplexVertex[] vertices   = simplex.m_vertices;
            const int         k_maxIters = 20;

            // These store the vertices of the last simplex so that we
            // can check for duplicates and preven cycling
            int[] saveA     = s_saveA;
            int[] saveB     = s_saveB;
            int   saveCount = 0;

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

            int    i;
            b2Vec2 p;

            // Main iteration loop
            int iter = 0;

            while (iter < k_maxIters)
            {
                // Copy the simplex so that we can identify duplicates
                saveCount = simplex.m_count;
                for (i = 0; i < saveCount; i++)
                {
                    saveA[i] = vertices[i].indexA;
                    saveB[i] = vertices[i].indexB;
                }

                /*switch(simplex.m_count)
                 * {
                 *      case 1:
                 *              break;
                 *      case 2:
                 *              simplex.Solve2();
                 *              break;
                 *      case 3:
                 *              simplex.Solve3();
                 *              break;
                 *      default:
                 *              b2Settings.b2Assert(false);
                 * }*/
                if (simplex.m_count == 1)
                {
                }
                else if (simplex.m_count == 2)
                {
                    simplex.Solve2();
                }
                else if (simplex.m_count == 3)
                {
                    simplex.Solve3();
                }
                else
                {
                    b2Settings.b2Assert(false);
                }

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

                // Compute the closest point.
                p            = simplex.GetClosestPoint();
                distanceSqr2 = p.LengthSquared();

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

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

                // Ensure the search direction is numerically fit.
                if (d.LengthSquared() < 0.0f /*Number.MinValue * Number.MinValue*/)
                {
                    // 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 very 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
                b2SimplexVertex vertex = vertices[simplex.m_count];
                vertex.indexA = (int)proxyA.GetSupport(b2Math.MulTMV(transformA.R, d.GetNegative()));
                vertex.wA     = b2Math.MulX(transformA, proxyA.GetVertex(vertex.indexA));
                vertex.indexB = (int)proxyB.GetSupport(b2Math.MulTMV(transformB.R, d));
                vertex.wB     = b2Math.MulX(transformB, proxyB.GetVertex(vertex.indexB));
                vertex.w      = b2Math.SubtractVV(vertex.wB, vertex.wA);

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

                // Check for duplicate support points. This is the main termination criteria.
                bool duplicate = false;
                for (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 exist to avoid cycling
                if (duplicate)
                {
                    break;
                }

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

            b2_gjkMaxIters = (int)b2Math.Max(b2_gjkMaxIters, iter);

            // Prepare output
            simplex.GetWitnessPoints(output.pointA, output.pointB);
            output.distance   = b2Math.SubtractVV(output.pointA, output.pointB).Length();
            output.iterations = iter;

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

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

                if (output.distance > rA + rB && output.distance > float.MinValue)
                {
                    // Shapes are still not overlapped.
                    // Move the witness points to the outer surface.
                    output.distance -= rA + rB;
                    b2Vec2 normal = b2Math.SubtractVV(output.pointB, output.pointA);
                    normal.Normalize();
                    output.pointA.x += rA * normal.x;
                    output.pointA.y += rA * normal.y;
                    output.pointB.x -= rB * normal.x;
                    output.pointB.y -= rB * normal.y;
                }
                else
                {
                    // Shapes are overlapped when radii are considered.
                    // Move the witness points to the middle.
                    p               = new b2Vec2();
                    p.x             = 0.5f * (output.pointA.x + output.pointB.x);
                    p.y             = 0.5f * (output.pointA.y + output.pointB.y);
                    output.pointA.x = output.pointB.x = p.x;
                    output.pointA.y = output.pointB.y = p.y;
                    output.distance = 0.0f;
                }
            }
        }
示例#2
0
		public static void b2Distance(out b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
		{
			++b2DistanceProxy.b2_gjkCalls;

			b2DistanceProxy proxyA = input.proxyA;
			b2DistanceProxy proxyB = input.proxyB;
			
			// Initialize the simplex.
			b2Simplex simplex = new b2Simplex();
            simplex.ReadCache(ref cache, proxyA, ref input.transformA, proxyB, ref input.transformB);
			
			// Get simplex vertices as an array.
            b2SimplexVertex[] vertices = b2Simplex.m_vertices;
			int k_maxIters = 20;
			
			// These store the vertices of the last simplex so that we
			// can check for duplicates and prevent cycling.
            int[] saveA = _saveA;
            int[] saveB = _saveB;
			int saveCount = 0;

		    b2Vec2 closestPoint;
            simplex.GetClosestPoint(out closestPoint);
			float distanceSqr1 = closestPoint.LengthSquared;
			float distanceSqr2 = distanceSqr1;
			
//            Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);


			// Main iteration loop.
            #region Main Iteration Loop
			int iter = 0;
			while (iter < k_maxIters)
			{
				// Copy simplex so we can identify duplicates.
				saveCount = simplex.m_count;
				for (int i = 0; i < saveCount; ++i)
				{
					saveA[i] = vertices[i].indexA;
					saveB[i] = vertices[i].indexB;
				}
				
				switch (simplex.m_count)
				{
				case 1:
					break;
					
				case 2:
					simplex.Solve2();
					break;
					
				case 3:
					simplex.Solve3();
					break;
					
				default:
					Debug.Assert(false);
					break;
				}
				
				// If we have 3 points, then the origin is in the corresponding triangle.
				if (simplex.m_count == 3)
				{
					break;
				}
				
				// Compute closest point.
			    b2Vec2 p;
                simplex.GetClosestPoint(out p);
			    distanceSqr2 = p.x * p.x + p.y * p.y;

			    // Ensure progress
				if (distanceSqr2 >= distanceSqr1)
				{
					//break;
				}
				distanceSqr1 = distanceSqr2;
				
				// Get search direction.
			    b2Vec2 d;
                simplex.GetSearchDirection(out d);
				
				// Ensure the search direction is numerically fit.
				if ((d.x * d.x + d.y * d.y) < b2Settings.b2_epsilon * b2Settings.b2_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.
				b2SimplexVertex vertex = vertices[simplex.m_count];

			    var q = input.transformA.q;

                b2Vec2 b;
                b.x = q.c * -d.x + q.s * -d.y;
                b.y = -q.s * -d.x + q.c * -d.y;

                vertex.indexA = proxyA.GetSupport(ref b);

			    var vA = proxyA.m_vertices[vertex.indexA];

                vertex.wA.x = (q.c * vA.x - q.s * vA.y) + input.transformA.p.x;
                vertex.wA.y = (q.s * vA.x + q.c * vA.y) + input.transformA.p.y;

			    //                b2Vec2 wBLocal = new b2Vec2();
			    q = input.transformB.q;
                b.x = q.c * d.x + q.s * d.y;
                b.y = -q.s * d.x + q.c * d.y;

			    vertex.indexB = proxyB.GetSupport(ref b);

			    var vB = proxyB.m_vertices[vertex.indexB];

                vertex.wB.x = (input.transformB.q.c * vB.x - input.transformB.q.s * vB.y) + input.transformB.p.x;
                vertex.wB.y = (input.transformB.q.s * vB.x + input.transformB.q.c * vB.y) + input.transformB.p.y;

                vertex.w.x = vertex.wB.x - vertex.wA.x;
                vertex.w.y = vertex.wB.y - vertex.wA.y;
				
				// Iteration count is equated to the number of support point calls.
				++iter;
				++b2DistanceProxy.b2_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.m_count;
			}
            #endregion
			
			b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);
			
			// Prepare output.
            simplex.GetWitnessPoints(out output.pointA, out output.pointB);
			output.distance = b2Math.b2Distance(ref output.pointA, ref output.pointB);
			output.iterations = iter;
			
			// Cache the simplex.
			simplex.WriteCache(ref cache);
			
			// Apply radii if requested.
			if (input.useRadii)
			{
				float rA = proxyA.Radius;
				float rB = proxyB.Radius;
				
				if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
				{
					// Shapes are still not overlapped.
					// Move the witness points to the outer surface.
					output.distance -= rA + rB;
					b2Vec2 normal;
                    normal.x = output.pointB.x - output.pointA.x;
                    normal.y = output.pointB.y - output.pointA.y;
                    
                    normal.Normalize();

                    output.pointA.x += rA * normal.x;
                    output.pointA += rA * normal;

                    output.pointB.x -= rB * normal.x;
                    output.pointB.y -= rB * normal.y;
                }
				else
				{
					// Shapes are overlapped when radii are considered.
					// Move the witness points to the middle.
					b2Vec2 p;
                    p.x = 0.5f * (output.pointA.x + output.pointB.x);
                    p.y = 0.5f * (output.pointA.y + output.pointB.y);
                    
                    output.pointA = p;
					output.pointB = p;
					output.distance = 0.0f;
				}
			}
		}
示例#3
0
        public static void b2Distance(out b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
        {
            ++b2DistanceProxy.b2_gjkCalls;

            b2DistanceProxy proxyA = input.proxyA;
            b2DistanceProxy proxyB = input.proxyB;

            // Initialize the simplex.
            b2Simplex simplex = new b2Simplex();

            simplex.ReadCache(ref cache, proxyA, ref input.transformA, proxyB, ref input.transformB);

            // Get simplex vertices as an array.
            b2SimplexVertex[] vertices = b2Simplex.m_vertices;
            int k_maxIters             = 20;

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

            b2Vec2 closestPoint;

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

//            Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);


            // Main iteration loop.
            #region Main Iteration Loop
            int iter = 0;
            while (iter < k_maxIters)
            {
                // Copy simplex so we can identify duplicates.
                saveCount = simplex.m_count;
                for (int i = 0; i < saveCount; ++i)
                {
                    saveA[i] = vertices[i].indexA;
                    saveB[i] = vertices[i].indexB;
                }

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

                case 2:
                    simplex.Solve2();
                    break;

                case 3:
                    simplex.Solve3();
                    break;

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

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

                // Compute closest point.
                b2Vec2 p;
                simplex.GetClosestPoint(out p);
                distanceSqr2 = p.x * p.x + p.y * p.y;

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

                // Get search direction.
                b2Vec2 d;
                simplex.GetSearchDirection(out d);

                // Ensure the search direction is numerically fit.
                if ((d.x * d.x + d.y * d.y) < b2Settings.b2_epsilon * b2Settings.b2_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.
                b2SimplexVertex vertex = vertices[simplex.m_count];

                var q = input.transformA.q;

                b2Vec2 b;
                b.x = q.c * -d.x + q.s * -d.y;
                b.y = -q.s * -d.x + q.c * -d.y;

                vertex.indexA = proxyA.GetSupport(ref b);

                var vA = proxyA.m_vertices[vertex.indexA];

                vertex.wA.x = (q.c * vA.x - q.s * vA.y) + input.transformA.p.x;
                vertex.wA.y = (q.s * vA.x + q.c * vA.y) + input.transformA.p.y;

                //                b2Vec2 wBLocal = new b2Vec2();
                q   = input.transformB.q;
                b.x = q.c * d.x + q.s * d.y;
                b.y = -q.s * d.x + q.c * d.y;

                vertex.indexB = proxyB.GetSupport(ref b);

                var vB = proxyB.m_vertices[vertex.indexB];

                vertex.wB.x = (input.transformB.q.c * vB.x - input.transformB.q.s * vB.y) + input.transformB.p.x;
                vertex.wB.y = (input.transformB.q.s * vB.x + input.transformB.q.c * vB.y) + input.transformB.p.y;

                vertex.w.x = vertex.wB.x - vertex.wA.x;
                vertex.w.y = vertex.wB.y - vertex.wA.y;

                // Iteration count is equated to the number of support point calls.
                ++iter;
                ++b2DistanceProxy.b2_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.m_count;
            }
            #endregion

            b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);

            // Prepare output.
            simplex.GetWitnessPoints(out output.pointA, out output.pointB);
            output.distance   = b2Math.b2Distance(ref output.pointA, ref output.pointB);
            output.iterations = iter;

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

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

                if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
                {
                    // Shapes are still not overlapped.
                    // Move the witness points to the outer surface.
                    output.distance -= rA + rB;
                    b2Vec2 normal;
                    normal.x = output.pointB.x - output.pointA.x;
                    normal.y = output.pointB.y - output.pointA.y;

                    normal.Normalize();

                    output.pointA.x += rA * normal.x;
                    output.pointA   += rA * normal;

                    output.pointB.x -= rB * normal.x;
                    output.pointB.y -= rB * normal.y;
                }
                else
                {
                    // Shapes are overlapped when radii are considered.
                    // Move the witness points to the middle.
                    b2Vec2 p;
                    p.x = 0.5f * (output.pointA.x + output.pointB.x);
                    p.y = 0.5f * (output.pointA.y + output.pointB.y);

                    output.pointA   = p;
                    output.pointB   = p;
                    output.distance = 0.0f;
                }
            }
        }
示例#4
0
        public static void b2Distance(ref b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
        {
            ++b2DistanceProxy.b2_gjkCalls;

            b2DistanceProxy proxyA = input.proxyA.Copy();
            b2DistanceProxy proxyB = input.proxyB.Copy();

            b2Transform transformA = input.transformA;
            b2Transform transformB = input.transformB;

            // Initialize the simplex.
            b2Simplex simplex = new b2Simplex();

            simplex.ReadCache(ref cache, ref proxyA, ref transformA, ref proxyB, ref transformB);

            // Get simplex vertices as an array.
            b2SimplexVertex[] vertices = new b2SimplexVertex[] { simplex.m_vertices[0], simplex.m_vertices[1], simplex.m_vertices[2] };
            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];
            int[] saveB     = new int[3];
            int   saveCount = 0;

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

//            Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);


            // Main iteration loop.
            #region Main Iteration Loop
            int iter = 0;
            while (iter < k_maxIters)
            {
                // Copy simplex so we can identify duplicates.
                saveCount = simplex.m_count;
                for (int i = 0; i < saveCount; ++i)
                {
                    saveA[i] = vertices[i].indexA;
                    saveB[i] = vertices[i].indexB;
                }

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

                case 2:
                    simplex.Solve2();
                    break;

                case 3:
                    simplex.Solve3();
                    break;

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

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

                // Compute closest point.
                b2Vec2 p = simplex.GetClosestPoint();
                distanceSqr2 = p.LengthSquared();

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

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

                // Ensure the search direction is numerically fit.
                if (d.LengthSquared() < b2Settings.b2_epsilon * b2Settings.b2_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.
                b2SimplexVertex vertex = vertices[simplex.m_count];
                vertex.indexA = proxyA.GetSupport(b2Math.b2MulT(transformA.q, -d));
                vertex.wA     = b2Math.b2Mul(transformA, proxyA.GetVertex(vertex.indexA));
                //                b2Vec2 wBLocal = new b2Vec2();
                vertex.indexB             = proxyB.GetSupport(b2Math.b2MulT(transformB.q, d));
                vertex.wB                 = b2Math.b2Mul(transformB, proxyB.GetVertex(vertex.indexB));
                vertex.w                  = vertex.wB - vertex.wA;
                vertices[simplex.m_count] = vertex;

                // Iteration count is equated to the number of support point calls.
                ++iter;
                ++b2DistanceProxy.b2_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.m_count;
            }
            #endregion

            b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);

            // Prepare output.
            simplex.GetWitnessPoints(ref output.pointA, ref output.pointB);
            output.distance   = b2Math.b2Distance(output.pointA, output.pointB);
            output.iterations = iter;

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

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

                if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
                {
                    // Shapes are still not overlapped.
                    // Move the witness points to the outer surface.
                    output.distance -= rA + rB;
                    b2Vec2 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.
                    b2Vec2 p = 0.5f * (output.pointA + output.pointB);
                    output.pointA   = p;
                    output.pointB   = p;
                    output.distance = 0.0f;
                }
            }
            // Copy back the vertex changes because they are structs, but in C++ land they are reference values
            simplex.m_vertices[0] = vertices[0];
            simplex.m_vertices[1] = vertices[1];
            simplex.m_vertices[2] = vertices[2];
        }
示例#5
0
        public static void b2Distance(ref b2DistanceOutput output, ref b2SimplexCache cache, ref b2DistanceInput input)
        {
            ++b2DistanceProxy.b2_gjkCalls;

            b2DistanceProxy proxyA = input.proxyA.Copy();
            b2DistanceProxy proxyB = input.proxyB.Copy();

            b2Transform transformA = input.transformA;
            b2Transform transformB = input.transformB;

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

            // Get simplex vertices as an array.
            b2SimplexVertex[] vertices = new b2SimplexVertex[] { simplex.m_vertices[0], simplex.m_vertices[1], simplex.m_vertices[2] };
            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];
            int[] saveB = new int[3];
            int saveCount = 0;

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

            //            Console.WriteLine("Closest Point={0},{1}, distance={2}", closestPoint.x, closestPoint.y, distanceSqr1);

            // Main iteration loop.
            #region Main Iteration Loop
            int iter = 0;
            while (iter < k_maxIters)
            {
                // Copy simplex so we can identify duplicates.
                saveCount = simplex.m_count;
                for (int i = 0; i < saveCount; ++i)
                {
                    saveA[i] = vertices[i].indexA;
                    saveB[i] = vertices[i].indexB;
                }

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

                case 2:
                    simplex.Solve2();
                    break;

                case 3:
                    simplex.Solve3();
                    break;

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

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

                // Compute closest point.
                b2Vec2 p = simplex.GetClosestPoint();
                distanceSqr2 = p.LengthSquared();

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

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

                // Ensure the search direction is numerically fit.
                if (d.LengthSquared() < b2Settings.b2_epsilon * b2Settings.b2_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.
                b2SimplexVertex vertex = vertices[simplex.m_count];
                vertex.indexA = proxyA.GetSupport(b2Math.b2MulT(transformA.q, -d));
                vertex.wA = b2Math.b2Mul(transformA, proxyA.GetVertex(vertex.indexA));
                //                b2Vec2 wBLocal = new b2Vec2();
                vertex.indexB = proxyB.GetSupport(b2Math.b2MulT(transformB.q, d));
                vertex.wB = b2Math.b2Mul(transformB, proxyB.GetVertex(vertex.indexB));
                vertex.w = vertex.wB - vertex.wA;
                vertices[simplex.m_count] = vertex;

                // Iteration count is equated to the number of support point calls.
                ++iter;
                ++b2DistanceProxy.b2_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.m_count;
            }
            #endregion

            b2DistanceProxy.b2_gjkMaxIters = Math.Max(b2DistanceProxy.b2_gjkMaxIters, iter);

            // Prepare output.
            simplex.GetWitnessPoints(ref output.pointA, ref output.pointB);
            output.distance = b2Math.b2Distance(output.pointA, output.pointB);
            output.iterations = iter;

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

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

                if (output.distance > rA + rB && output.distance > b2Settings.b2_epsilon)
                {
                    // Shapes are still not overlapped.
                    // Move the witness points to the outer surface.
                    output.distance -= rA + rB;
                    b2Vec2 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.
                    b2Vec2 p = 0.5f * (output.pointA + output.pointB);
                    output.pointA = p;
                    output.pointB = p;
                    output.distance = 0.0f;
                }
            }
            // Copy back the vertex changes because they are structs, but in C++ land they are reference values
            simplex.m_vertices[0] = vertices[0];
            simplex.m_vertices[1] = vertices[1];
            simplex.m_vertices[2] = vertices[2];
        }