Example #1
0
 internal void WriteCache(ref SimplexCache cache)
 {
     cache.metric = GetMetric();
     cache.count  = (UInt16)_count;
     for (int i = 0; i < _count; ++i)
     {
         cache.indexA[i] = (byte)(_v[i].indexA);
         cache.indexB[i] = (byte)(_v[i].indexB);
     }
 }
Example #2
0
        internal void ReadCache(ref SimplexCache cache,
                                Shape shapeA, ref XForm transformA,
                                Shape shapeB, ref XForm transformB)
        {
            Debug.Assert(0 <= cache.count && cache.count <= 3);

            // Copy data from cache.
            _count = cache.count;
            for (int i = 0; i < _count; ++i)
            {
                SimplexVertex v = _v[i];
                v.indexA = cache.indexA[i];
                v.indexB = cache.indexB[i];
                Vector2 wALocal = shapeA.GetVertex(v.indexA);
                Vector2 wBLocal = shapeB.GetVertex(v.indexB);
                v.wA  = MathUtils.Multiply(ref transformA, wALocal);
                v.wB  = MathUtils.Multiply(ref transformB, wBLocal);
                v.w   = v.wB - v.wA;
                v.a   = 0.0f;
                _v[i] = v;
            }

            // Compute the new simplex metric, if it is substantially different than
            // old metric then flush the simplex.
            if (_count > 1)
            {
                float metric1 = cache.metric;
                float metric2 = GetMetric();
                if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.b2_FLT_EPSILON)
                {
                    // Reset the simplex.
                    _count = 0;
                }
            }

            // If the cache is empty or invalid ...
            if (_count == 0)
            {
                SimplexVertex v = _v[0];
                v.indexA = 0;
                v.indexB = 0;
                Vector2 wALocal = shapeA.GetVertex(0);
                Vector2 wBLocal = shapeB.GetVertex(0);
                v.wA   = MathUtils.Multiply(ref transformA, wALocal);
                v.wB   = MathUtils.Multiply(ref transformB, wBLocal);
                v.w    = v.wB - v.wA;
                _v[0]  = v;
                _count = 1;
            }
        }
Example #3
0
        public SeparationFunction(ref SimplexCache cache,
		    Shape shapeA, ref XForm transformA,
            Shape shapeB, ref XForm transformB)
        {
            _localPoint = Vector2.Zero;
            _shapeA = shapeA;
            _shapeB = shapeB;
            int count = cache.count;
            Debug.Assert(0 < count && count < 3);

            if (count == 1)
            {
                _type = SeparationFunctionType.Points;
                Vector2 localPointA = _shapeA.GetVertex(cache.indexA[0]);
                Vector2 localPointB = _shapeB.GetVertex(cache.indexB[0]);
                Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
                Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);
                _axis = pointB - pointA;
                _axis.Normalize();
            }
            else if (cache.indexB[0] == cache.indexB[1])
            {
                // Two points on A and one on B
                _type = SeparationFunctionType.FaceA;
                Vector2 localPointA1 = _shapeA.GetVertex(cache.indexA[0]);
                Vector2 localPointA2 = _shapeA.GetVertex(cache.indexA[1]);
                Vector2 localPointB = _shapeB.GetVertex(cache.indexB[0]);
                _localPoint = 0.5f * (localPointA1 + localPointA2);
                _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
                _axis.Normalize();

                Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis);
                Vector2 pointA = MathUtils.Multiply(ref transformA, _localPoint);
                Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);

                float s = Vector2.Dot(pointB - pointA, normal);
                if (s < 0.0f)
                {
                    _axis = -_axis;
                }
            }
            else if (cache.indexA[0] == cache.indexA[1])
            {
                // Two points on B and one on A.
                _type = SeparationFunctionType.FaceB;
                Vector2 localPointA = shapeA.GetVertex(cache.indexA[0]);
                Vector2 localPointB1 = shapeB.GetVertex(cache.indexB[0]);
                Vector2 localPointB2 = shapeB.GetVertex(cache.indexB[1]);
                _localPoint = 0.5f * (localPointB1 + localPointB2);
                _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
                _axis.Normalize();

                Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis);
                Vector2 pointB = MathUtils.Multiply(ref transformB, _localPoint);
                Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);

                float s = Vector2.Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    _axis = -_axis;
                }
            }
            else
            {
                // Two points on B and two points on A.
                // The faces are parallel.
                Vector2 localPointA1 = _shapeA.GetVertex(cache.indexA[0]);
                Vector2 localPointA2 = _shapeA.GetVertex(cache.indexA[1]);
                Vector2 localPointB1 = _shapeB.GetVertex(cache.indexB[0]);
                Vector2 localPointB2 = _shapeB.GetVertex(cache.indexB[1]);

                Vector2 pA = MathUtils.Multiply(ref transformA, localPointA1);
                Vector2 dA = MathUtils.Multiply(ref transformA.R, localPointA2 - localPointA1);
                Vector2 pB = MathUtils.Multiply(ref transformB, localPointB1);
                Vector2 dB = MathUtils.Multiply(ref transformB.R, localPointB2 - localPointB1);

                float a = Vector2.Dot(dA, dA);
                float e = Vector2.Dot(dB, dB);
                Vector2 r = pA - pB;
                float c = Vector2.Dot(dA, r);
                float f = Vector2.Dot(dB, r);

                float b = Vector2.Dot(dA, dB);
                float denom = a * e - b * b;

                float s = 0.0f;
                if (denom != 0.0f)
                {
                    s = MathUtils.Clamp((b * f - c * e) / denom, 0.0f, 1.0f);
                }

                float t = (b * s + f) / e;

                if (t < 0.0f)
                {
                    t = 0.0f;
                    s = MathUtils.Clamp(-c / a, 0.0f, 1.0f);
                }
                else if (t > 1.0f)
                {
                    t = 1.0f;
                    s = MathUtils.Clamp((b - c) / a, 0.0f, 1.0f);
                }

                Vector2 localPointA = localPointA1 + s * (localPointA2 - localPointA1);
                Vector2 localPointB = localPointB1 + t * (localPointB2 - localPointB1);

                if (s == 0.0f || s == 1.0f)
                {
                    _type = SeparationFunctionType.FaceB;
                    _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
                    _axis.Normalize();

                    _localPoint = localPointB;

                    Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis);
                    Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
                    Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);

                    float sgn = Vector2.Dot(pointA - pointB, normal);
                    if (sgn < 0.0f)
                    {
                        _axis = -_axis;
                    }
                }
                else
                {
                    _type = SeparationFunctionType.FaceA;
                    _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
                    _axis.Normalize();

                    _localPoint = localPointA;

                    Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis);
                    Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
                    Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);

                    float sgn = Vector2.Dot(pointB - pointA, normal);
                    if (sgn < 0.0f)
                    {
                        _axis = -_axis;
                    }
                }
            }
        }
Example #4
0
        public static void ComputeDistance(out DistanceOutput output,
				        out SimplexCache cache,
				        ref DistanceInput input,
				        Shape shapeA,
                        Shape shapeB)
        {
            cache = new SimplexCache();
            ++b2_gjkCalls;

            XForm transformA = input.transformA;
            XForm transformB = input.transformB;

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

            // Get simplex vertices as an array.
            int k_maxIters = 20;

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
            FixedArray3<int> saveA = new FixedArray3<int>();
            FixedArray3<int> saveB = new FixedArray3<int>();
            int saveCount = 0;

            Vector2 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] = simplex._v[i].indexA;
                    saveB[i] = simplex._v[i].indexB;
                }

                switch (simplex._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._count == 3)
                {
                    break;
                }

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

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

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

                // Ensure the search direction is numerically fit.
                if (d.LengthSquared() < Settings.b2_FLT_EPSILON * Settings.b2_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 = simplex._v[simplex._count];
                vertex.indexA = shapeA.GetSupport(MathUtils.MultiplyT(ref transformA.R, -d));
                vertex.wA = MathUtils.Multiply(ref transformA, shapeA.GetVertex(vertex.indexA));
                //Vector2 wBLocal;
                vertex.indexB = shapeB.GetSupport(MathUtils.MultiplyT(ref transformB.R, d));
                vertex.wB = MathUtils.Multiply(ref transformB, shapeB.GetVertex(vertex.indexB));
                vertex.w = vertex.wB - vertex.wA;
                simplex._v[simplex._count] = vertex;

                // 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 (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;
            }

            b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter);

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

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

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

                if (output.distance > rA + rB && output.distance > Settings.b2_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;
                }
            }
        }
Example #5
0
 internal void WriteCache(ref SimplexCache cache)
 {
     cache.metric = GetMetric();
     cache.count = (UInt16)_count;
     for (int i = 0; i < _count; ++i)
     {
         cache.indexA[i] = (byte)(_v[i].indexA);
         cache.indexB[i] = (byte)(_v[i].indexB);
     }
 }
Example #6
0
        internal void ReadCache(	ref SimplexCache cache,
					    Shape shapeA, ref XForm transformA,
                        Shape shapeB, ref XForm transformB)
        {
            Debug.Assert(0 <= cache.count && cache.count <= 3);

            // Copy data from cache.
            _count = cache.count;
            for (int i = 0; i < _count; ++i)
            {
                SimplexVertex v = _v[i];
                v.indexA = cache.indexA[i];
                v.indexB = cache.indexB[i];
                Vector2 wALocal = shapeA.GetVertex(v.indexA);
                Vector2 wBLocal = shapeB.GetVertex(v.indexB);
                v.wA = MathUtils.Multiply(ref transformA, wALocal);
                v.wB = MathUtils.Multiply(ref transformB, wBLocal);
                v.w = v.wB - v.wA;
                v.a = 0.0f;
                _v[i] = v;
            }

            // Compute the new simplex metric, if it is substantially different than
            // old metric then flush the simplex.
            if (_count > 1)
            {
                float metric1 = cache.metric;
                float metric2 = GetMetric();
                if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.b2_FLT_EPSILON)
                {
                    // Reset the simplex.
                    _count = 0;
                }
            }

            // If the cache is empty or invalid ...
            if (_count == 0)
            {
                SimplexVertex v = _v[0];
                v.indexA = 0;
                v.indexB = 0;
                Vector2 wALocal = shapeA.GetVertex(0);
                Vector2 wBLocal = shapeB.GetVertex(0);
                v.wA = MathUtils.Multiply(ref transformA, wALocal);
                v.wB = MathUtils.Multiply(ref transformB, wBLocal);
                v.w = v.wB - v.wA;
                _v[0] = v;
                _count = 1;
            }
        }
Example #7
0
        public SeparationFunction(ref SimplexCache cache,
                                  Shape shapeA, ref XForm transformA,
                                  Shape shapeB, ref XForm transformB)
        {
            _localPoint = Vector2.Zero;
            _shapeA     = shapeA;
            _shapeB     = shapeB;
            int count = cache.count;

            Debug.Assert(0 < count && count < 3);

            if (count == 1)
            {
                _type = SeparationFunctionType.Points;
                Vector2 localPointA = _shapeA.GetVertex(cache.indexA[0]);
                Vector2 localPointB = _shapeB.GetVertex(cache.indexB[0]);
                Vector2 pointA      = MathUtils.Multiply(ref transformA, localPointA);
                Vector2 pointB      = MathUtils.Multiply(ref transformB, localPointB);
                _axis = pointB - pointA;
                _axis.Normalize();
            }
            else if (cache.indexB[0] == cache.indexB[1])
            {
                // Two points on A and one on B
                _type = SeparationFunctionType.FaceA;
                Vector2 localPointA1 = _shapeA.GetVertex(cache.indexA[0]);
                Vector2 localPointA2 = _shapeA.GetVertex(cache.indexA[1]);
                Vector2 localPointB  = _shapeB.GetVertex(cache.indexB[0]);
                _localPoint = 0.5f * (localPointA1 + localPointA2);
                _axis       = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
                _axis.Normalize();

                Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis);
                Vector2 pointA = MathUtils.Multiply(ref transformA, _localPoint);
                Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);

                float s = Vector2.Dot(pointB - pointA, normal);
                if (s < 0.0f)
                {
                    _axis = -_axis;
                }
            }
            else if (cache.indexA[0] == cache.indexA[1])
            {
                // Two points on B and one on A.
                _type = SeparationFunctionType.FaceB;
                Vector2 localPointA  = shapeA.GetVertex(cache.indexA[0]);
                Vector2 localPointB1 = shapeB.GetVertex(cache.indexB[0]);
                Vector2 localPointB2 = shapeB.GetVertex(cache.indexB[1]);
                _localPoint = 0.5f * (localPointB1 + localPointB2);
                _axis       = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
                _axis.Normalize();

                Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis);
                Vector2 pointB = MathUtils.Multiply(ref transformB, _localPoint);
                Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);

                float s = Vector2.Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    _axis = -_axis;
                }
            }
            else
            {
                // Two points on B and two points on A.
                // The faces are parallel.
                Vector2 localPointA1 = _shapeA.GetVertex(cache.indexA[0]);
                Vector2 localPointA2 = _shapeA.GetVertex(cache.indexA[1]);
                Vector2 localPointB1 = _shapeB.GetVertex(cache.indexB[0]);
                Vector2 localPointB2 = _shapeB.GetVertex(cache.indexB[1]);

                Vector2 pA = MathUtils.Multiply(ref transformA, localPointA1);
                Vector2 dA = MathUtils.Multiply(ref transformA.R, localPointA2 - localPointA1);
                Vector2 pB = MathUtils.Multiply(ref transformB, localPointB1);
                Vector2 dB = MathUtils.Multiply(ref transformB.R, localPointB2 - localPointB1);

                float   a = Vector2.Dot(dA, dA);
                float   e = Vector2.Dot(dB, dB);
                Vector2 r = pA - pB;
                float   c = Vector2.Dot(dA, r);
                float   f = Vector2.Dot(dB, r);

                float b     = Vector2.Dot(dA, dB);
                float denom = a * e - b * b;

                float s = 0.0f;
                if (denom != 0.0f)
                {
                    s = MathUtils.Clamp((b * f - c * e) / denom, 0.0f, 1.0f);
                }

                float t = (b * s + f) / e;

                if (t < 0.0f)
                {
                    t = 0.0f;
                    s = MathUtils.Clamp(-c / a, 0.0f, 1.0f);
                }
                else if (t > 1.0f)
                {
                    t = 1.0f;
                    s = MathUtils.Clamp((b - c) / a, 0.0f, 1.0f);
                }

                Vector2 localPointA = localPointA1 + s * (localPointA2 - localPointA1);
                Vector2 localPointB = localPointB1 + t * (localPointB2 - localPointB1);

                if (s == 0.0f || s == 1.0f)
                {
                    _type = SeparationFunctionType.FaceB;
                    _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
                    _axis.Normalize();

                    _localPoint = localPointB;

                    Vector2 normal = MathUtils.Multiply(ref transformB.R, _axis);
                    Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
                    Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);

                    float sgn = Vector2.Dot(pointA - pointB, normal);
                    if (sgn < 0.0f)
                    {
                        _axis = -_axis;
                    }
                }
                else
                {
                    _type = SeparationFunctionType.FaceA;
                    _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
                    _axis.Normalize();

                    _localPoint = localPointA;

                    Vector2 normal = MathUtils.Multiply(ref transformA.R, _axis);
                    Vector2 pointA = MathUtils.Multiply(ref transformA, localPointA);
                    Vector2 pointB = MathUtils.Multiply(ref transformB, localPointB);

                    float sgn = Vector2.Dot(pointB - pointA, normal);
                    if (sgn < 0.0f)
                    {
                        _axis = -_axis;
                    }
                }
            }
        }
Example #8
0
        public static void ComputeDistance(out DistanceOutput output,
                                           out SimplexCache cache,
                                           ref DistanceInput input,
                                           Shape shapeA,
                                           Shape shapeB)
        {
            cache = new SimplexCache();
            ++b2_gjkCalls;

            XForm transformA = input.transformA;
            XForm transformB = input.transformB;

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

            simplex.ReadCache(ref cache, shapeA, ref transformA, shapeB, ref transformB);

            // Get simplex vertices as an array.
            int k_maxIters = 20;

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
            FixedArray3 <int> saveA = new FixedArray3 <int>();
            FixedArray3 <int> saveB = new FixedArray3 <int>();
            int saveCount           = 0;

            Vector2 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] = simplex._v[i].indexA;
                    saveB[i] = simplex._v[i].indexB;
                }

                switch (simplex._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._count == 3)
                {
                    break;
                }

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

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

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

                // Ensure the search direction is numerically fit.
                if (d.LengthSquared() < Settings.b2_FLT_EPSILON * Settings.b2_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 = simplex._v[simplex._count];
                vertex.indexA = shapeA.GetSupport(MathUtils.MultiplyT(ref transformA.R, -d));
                vertex.wA     = MathUtils.Multiply(ref transformA, shapeA.GetVertex(vertex.indexA));
                //Vector2 wBLocal;
                vertex.indexB = shapeB.GetSupport(MathUtils.MultiplyT(ref transformB.R, d));
                vertex.wB     = MathUtils.Multiply(ref transformB, shapeB.GetVertex(vertex.indexB));
                vertex.w      = vertex.wB - vertex.wA;
                simplex._v[simplex._count] = vertex;

                // 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 (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;
            }

            b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter);

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

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

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

                if (output.distance > rA + rB && output.distance > Settings.b2_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;
                }
            }
        }