Example #1
0
		public void ReadCache(SimplexCache cache,
					   DistanceProxy proxyA, Transform transformA,
					   DistanceProxy proxyB, Transform transformB)
		{
			Utilities.Assert(cache.count <= 3);
		
			// Copy data from cache.
			m_count = cache.count;
			SimplexVertex[] vertices = this.verticies;
			for (int i = 0; i < m_count; ++i)
			{
			    SimplexVertex v = vertices[i];
			    v.indexA = cache.indexA[i];
			    v.indexB = cache.indexB[i];
			    Vec2 wALocal = proxyA.GetVertex(v.indexA);
			    Vec2 wBLocal = proxyB.GetVertex(v.indexB);
			    v.wA = Utilities.Mul(transformA, wALocal);
			    v.wB = Utilities.Mul(transformB, wBLocal);
			    v.w = v.wB - v.wA;
			    v.a = 0.0f;
			}

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

			// If the cache is empty or invalid ...
			if (m_count == 0)
			{
			    SimplexVertex v = vertices[0];
			    v.indexA = 0;
			    v.indexB = 0;
			    Vec2 wALocal = proxyA.GetVertex(0);
			    Vec2 wBLocal = proxyB.GetVertex(0);
			    v.wA = Utilities.Mul(transformA, wALocal);
			    v.wB = Utilities.Mul(transformB, wBLocal);
			    v.w = v.wB - v.wA;
			    v.a = 1.0f;
			    m_count = 1;
			}
		}
Example #2
0
        public void ReadCache(SimplexCache cache,
                              DistanceProxy proxyA, Transform transformA,
                              DistanceProxy proxyB, Transform transformB)
        {
            Utilities.Assert(cache.count <= 3);

            // Copy data from cache.
            m_count = cache.count;
            SimplexVertex[] vertices = this.verticies;
            for (int i = 0; i < m_count; ++i)
            {
                SimplexVertex v = vertices[i];
                v.indexA = cache.indexA[i];
                v.indexB = cache.indexB[i];
                Vec2 wALocal = proxyA.GetVertex(v.indexA);
                Vec2 wBLocal = proxyB.GetVertex(v.indexB);
                v.wA = Utilities.Mul(transformA, wALocal);
                v.wB = Utilities.Mul(transformB, wBLocal);
                v.w  = v.wB - v.wA;
                v.a  = 0.0f;
            }

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

            // If the cache is empty or invalid ...
            if (m_count == 0)
            {
                SimplexVertex v = vertices[0];
                v.indexA = 0;
                v.indexB = 0;
                Vec2 wALocal = proxyA.GetVertex(0);
                Vec2 wBLocal = proxyB.GetVertex(0);
                v.wA    = Utilities.Mul(transformA, wALocal);
                v.wB    = Utilities.Mul(transformB, wBLocal);
                v.w     = v.wB - v.wA;
                v.a     = 1.0f;
                m_count = 1;
            }
        }
Example #3
0
        internal void ReadCache(ref SimplexCache cache,
                                ref DistanceProxy proxyA, ref Transform transformA,
                                ref DistanceProxy proxyB, ref Transform transformB)
        {
            //Debug.Assert(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 = proxyA.GetVertex(v.indexA);
                Vector2 wBLocal = proxyB.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_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 = proxyA.GetVertex(0);
                Vector2 wBLocal = proxyB.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;
            }
        }
        // TODO_ERIN might not need to return the separation

        float Initialize(SimplexCache cache,
                         DistanceProxy proxyA, Sweep sweepA,
                         DistanceProxy proxyB, Sweep sweepB,
                         float t1)
        {
            m_proxyA = proxyA;
            m_proxyB = proxyB;
            int count = cache.count;

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

            m_sweepA = sweepA;
            m_sweepB = sweepB;

            Transform xfA, xfB;

            m_sweepA.GetTransform(out xfA, t1);
            m_sweepB.GetTransform(out xfB, t1);

            if (count == 1)
            {
                m_type = SeparationType.e_points;
                Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]);
                Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
                Vec2 pointA      = Utilities.Mul(xfA, localPointA);
                Vec2 pointB      = Utilities.Mul(xfB, localPointB);
                m_axis = pointB - pointA;
                float s = m_axis.Normalize();
                return(s);
            }
            else if (cache.indexA[0] == cache.indexA[1])
            {
                // Two points on B and one on A.
                m_type = SeparationType.e_faceB;
                Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
                Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);

                m_axis = Utilities.Cross(localPointB2 - localPointB1, 1.0f);
                m_axis.Normalize();
                Vec2 normal = Utilities.Mul(xfB.q, m_axis);

                m_localPoint = 0.5f * (localPointB1 + localPointB2);
                Vec2 pointB = Utilities.Mul(xfB, m_localPoint);

                Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]);
                Vec2 pointA      = Utilities.Mul(xfA, localPointA);

                float s = Utilities.Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    m_axis = -m_axis;
                    s      = -s;
                }
                return(s);
            }
            else
            {
                // Two points on A and one or two points on B.
                m_type = SeparationType.e_faceA;
                Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
                Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);

                m_axis = Utilities.Cross(localPointA2 - localPointA1, 1.0f);
                m_axis.Normalize();
                Vec2 normal = Utilities.Mul(xfA.q, m_axis);

                m_localPoint = 0.5f * (localPointA1 + localPointA2);
                Vec2 pointA = Utilities.Mul(xfA, m_localPoint);

                Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
                Vec2 pointB      = Utilities.Mul(xfB, localPointB);

                float s = Utilities.Dot(pointB - pointA, normal);
                if (s < 0.0f)
                {
                    m_axis = -m_axis;
                    s      = -s;
                }
                return(s);
            }
        }
Example #5
0
        public SeparationFunction(ref SimplexCache cache,
                                  ref DistanceProxy proxyA, ref Sweep sweepA,
                                  ref DistanceProxy proxyB, ref Sweep sweepB,
                                  float t1)
        {
            _localPoint = Vector2.zero;
            _proxyA     = proxyA;
            _proxyB     = proxyB;
            int count = cache.count;

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

            _sweepA = sweepA;
            _sweepB = sweepB;

            Transform xfA, xfB;

            _sweepA.GetTransform(out xfA, t1);
            _sweepB.GetTransform(out xfB, t1);

            if (count == 1)
            {
                _type = SeparationFunctionType.Points;
                Vector2 localPointA = _proxyA.GetVertex(cache.indexA[0]);
                Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]);
                Vector2 pointA      = MathUtils.Multiply(ref xfA, localPointA);
                Vector2 pointB      = MathUtils.Multiply(ref xfB, localPointB);
                _axis = pointB - pointA;
                _axis.Normalize();
                return;
            }
            else if (cache.indexA[0] == cache.indexA[1])
            {
                // Two points on B and one on A.
                _type = SeparationFunctionType.FaceB;
                Vector2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
                Vector2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);

                _axis = MathUtils.Cross(localPointB2 - localPointB1, 1.0f);
                _axis.Normalize();
                Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);

                _localPoint = 0.5f * (localPointB1 + localPointB2);
                Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);

                Vector2 localPointA = proxyA.GetVertex(cache.indexA[0]);
                Vector2 pointA      = MathUtils.Multiply(ref xfA, localPointA);

                float s = Vector2.Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    _axis = -_axis;
                    s     = -s;
                }
                return;
            }
            else
            {
                // Two points on A and one or two points on B.
                _type = SeparationFunctionType.FaceA;
                Vector2 localPointA1 = _proxyA.GetVertex(cache.indexA[0]);
                Vector2 localPointA2 = _proxyA.GetVertex(cache.indexA[1]);

                _axis = MathUtils.Cross(localPointA2 - localPointA1, 1.0f);
                _axis.Normalize();
                Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);

                _localPoint = 0.5f * (localPointA1 + localPointA2);
                Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);

                Vector2 localPointB = _proxyB.GetVertex(cache.indexB[0]);
                Vector2 pointB      = MathUtils.Multiply(ref xfB, localPointB);

                float s = Vector2.Dot(pointB - pointA, normal);
                if (s < 0.0f)
                {
                    _axis = -_axis;
                    s     = -s;
                }
                return;
            }
        }
Example #6
0
        /// Compute the closest points between two shapes. Supports any combination of:
        /// CircleShape, PolygonShape, EdgeShape. The simplex cache is input/output.
        /// On the first call set SimplexCache.count to zero.
        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, transformA, proxyB, transformB);

            // Get simplex vertices as an array.
            SimplexVertex[] vertices   = simplex.verticies;
            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];
            int[] saveB     = new int[3];
            int   saveCount = 0;

            float distanceSqr1 = Single.MaxValue;
            float distanceSqr2 = distanceSqr1;

            // 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:
                    Utilities.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.
                Vec2 p = simplex.GetClosestPoint();
                distanceSqr2 = 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() < Single.Epsilon * Single.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.m_count];
                vertex.indexA = proxyA.GetSupport(Utilities.MulT(transformA.q, -d));
                vertex.wA     = Utilities.Mul(transformA, proxyA.GetVertex(vertex.indexA));
                Vec2 wBLocal;
                vertex.indexB = proxyB.GetSupport(Utilities.MulT(transformB.q, d));
                vertex.wB     = Utilities.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.m_count;
            }

            _gjkMaxIters = Math.Max(_gjkMaxIters, iter);

            // Prepare output.
            simplex.GetWitnessPoints(out output.pointA, out output.pointB);
            output.distance   = Utilities.Distance(output.pointA, output.pointB);
            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 > Single.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;
                }
            }
        }
Example #7
0
        /// Compute the upper bound on time before two shapes penetrate. Time is represented as
        /// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
        /// non-tunneling collision. If you change the time interval, you should call this function
        /// again.
        /// Note: use Distance to compute the contact point and normal at the time of impact.
        // CCD via the local separating axis method. This seeks progression
        // by computing the largest time at which separation is maintained.
        public static void TimeOfImpact(out TOIOutput output, TOIInput input)
        {
            Timer timer = new Timer();

            ++_toiCalls;

            output.state = TOIOutput.State.e_unknown;
            output.t     = input.tMax;

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

            Sweep sweepA = input.sweepA;
            Sweep sweepB = input.sweepB;

            // Large rotations can make the root finder fail, so we normalize the
            // sweep angles.
            sweepA.Normalize();
            sweepB.Normalize();

            float tMax = input.tMax;

            float totalRadius = proxyA.m_radius + proxyB.m_radius;
            float target      = Math.Max(Settings._linearSlop, totalRadius - 3.0f * Settings._linearSlop);
            float tolerance   = 0.25f * Settings._linearSlop;

            Utilities.Assert(target > tolerance);

            float     t1 = 0.0f;
            const int k_maxIterations = 20;             // TODO_ERIN Settings
            int       iter            = 0;

            // Prepare input for distance query.
            SimplexCache cache = new SimplexCache();

            cache.count = 0;
            DistanceInput distanceInput;

            distanceInput.proxyA   = input.proxyA;
            distanceInput.proxyB   = input.proxyB;
            distanceInput.useRadii = false;

            // The outer loop progressively attempts to compute new separating axes.
            // This loop terminates when an axis is repeated (no progress is made).
            for (;;)
            {
                Transform xfA, xfB;
                sweepA.GetTransform(out xfA, t1);
                sweepB.GetTransform(out xfB, t1);

                // Get the distance between shapes. We can also use the results
                // to get a separating axis.
                distanceInput.transformA = xfA;
                distanceInput.transformB = xfB;
                DistanceOutput distanceOutput;
                Utilities.Distance(out distanceOutput, cache, distanceInput);

                // If the shapes are overlapped, we give up on continuous collision.
                if (distanceOutput.distance <= 0.0f)
                {
                    // Failure!
                    output.state = TOIOutput.State.e_overlapped;
                    output.t     = 0.0f;
                    break;
                }

                if (distanceOutput.distance < target + tolerance)
                {
                    // Victory!
                    output.state = TOIOutput.State.e_touching;
                    output.t     = t1;
                    break;
                }

                // Initialize the separating axis.
                throw new NotImplementedException();
                //        SeparationFunction fcn;
                //        fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
                //#if ZERO
                //        // Dump the curve seen by the root finder
                //        {
                //            const int N = 100;
                //            float dx = 1.0f / N;
                //            float xs[N+1];
                //            float fs[N+1];

                //            float x = 0.0f;

                //            for (int i = 0; i <= N; ++i)
                //            {
                //                sweepA.GetTransform(out xfA, x);
                //                sweepB.GetTransform(out xfB, x);
                //                float f = fcn.Evaluate(xfA, xfB) - target;

                //                printf("%g %g\n", x, f);

                //                xs[i] = x;
                //                fs[i] = f;

                //                x += dx;
                //            }
                //        }
                //#endif

                //        // Compute the TOI on the separating axis. We do this by successively
                //        // resolving the deepest point. This loop is bounded by the number of vertices.
                //        bool done = false;
                //        float t2 = tMax;
                //        int pushBackIter = 0;
                //        for (;;)
                //        {
                //            // Find the deepest point at t2. Store the witness point indices.
                //            int indexA, indexB;
                //            float s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);

                //            // Is the final configuration separated?
                //            if (s2 > target + tolerance)
                //            {
                //                // Victory!
                //                output.state = TOIOutput.State.e_separated;
                //                output.t = tMax;
                //                done = true;
                //                break;
                //            }

                //            // Has the separation reached tolerance?
                //            if (s2 > target - tolerance)
                //            {
                //                // Advance the sweeps
                //                t1 = t2;
                //                break;
                //            }

                //            // Compute the initial separation of the witness points.
                //            float s1 = fcn.Evaluate(indexA, indexB, t1);

                //            // Check for initial overlap. This might happen if the root finder
                //            // runs out of iterations.
                //            if (s1 < target - tolerance)
                //            {
                //                output.state = TOIOutput.State.e_failed;
                //                output.t = t1;
                //                done = true;
                //                break;
                //            }

                //            // Check for touching
                //            if (s1 <= target + tolerance)
                //            {
                //                // Victory! t1 should hold the TOI (could be 0.0).
                //                output.state = TOIOutput.State.e_touching;
                //                output.t = t1;
                //                done = true;
                //                break;
                //            }

                //            // Compute 1D root of: f(x) - target = 0
                //            int rootIterCount = 0;
                //            float a1 = t1, a2 = t2;
                //            for (;;)
                //            {
                //                // Use a mix of the secant rule and bisection.
                //                float t;
                //                if (rootIterCount & 1)
                //                {
                //                    // Secant rule to improve convergence.
                //                    t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
                //                }
                //                else
                //                {
                //                    // Bisection to guarantee progress.
                //                    t = 0.5f * (a1 + a2);
                //                }

                //                ++rootIterCount;
                //                ++_toiRootIters;

                //                float s = fcn.Evaluate(indexA, indexB, t);

                //                if (Math.Abs(s - target) < tolerance)
                //                {
                //                    // t2 holds a tentative value for t1
                //                    t2 = t;
                //                    break;
                //                }

                //                // Ensure we continue to bracket the root.
                //                if (s > target)
                //                {
                //                    a1 = t;
                //                    s1 = s;
                //                }
                //                else
                //                {
                //                    a2 = t;
                //                    s2 = s;
                //                }

                //                if (rootIterCount == 50)
                //                {
                //                    break;
                //                }
                //            }

                //            _toiMaxRootIters = Math.Max(_toiMaxRootIters, rootIterCount);

                //            ++pushBackIter;

                //            if (pushBackIter == Settings._maxPolygonVertices)
                //            {
                //                break;
                //            }
                //        }

                //        ++iter;
                //        ++_toiIters;

                //        if (done)
                //        {
                //            break;
                //        }

                //        if (iter == k_maxIterations)
                //        {
                //            // Root finder got stuck. Semi-victory.
                //            output.state = TOIOutput.State.e_failed;
                //            output.t = t1;
                //            break;
                //        }
            }

            _toiMaxIters = Math.Max(_toiMaxIters, iter);

            float time = timer.GetMilliseconds();

            _toiMaxTime = Math.Max(_toiMaxTime, time);
            _toiTime   += time;
        }
		// TODO_ERIN might not need to return the separation

		float Initialize(SimplexCache cache,
			DistanceProxy proxyA, Sweep sweepA,
			DistanceProxy proxyB, Sweep sweepB,
			float t1)
		{
			m_proxyA = proxyA;
			m_proxyB = proxyB;
			int count = cache.count;
			Utilities.Assert(0 < count && count < 3);

			m_sweepA = sweepA;
			m_sweepB = sweepB;

			Transform xfA, xfB;
			m_sweepA.GetTransform(out xfA, t1);
			m_sweepB.GetTransform(out xfB, t1);

			if (count == 1)
			{
				m_type = SeparationType.e_points;
				Vec2 localPointA = m_proxyA.GetVertex(cache.indexA[0]);
				Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
				Vec2 pointA = Utilities.Mul(xfA, localPointA);
				Vec2 pointB = Utilities.Mul(xfB, localPointB);
				m_axis = pointB - pointA;
				float s = m_axis.Normalize();
				return s;
			}
			else if (cache.indexA[0] == cache.indexA[1])
			{
				// Two points on B and one on A.
				m_type = SeparationType.e_faceB;
				Vec2 localPointB1 = proxyB.GetVertex(cache.indexB[0]);
				Vec2 localPointB2 = proxyB.GetVertex(cache.indexB[1]);

				m_axis = Utilities.Cross(localPointB2 - localPointB1, 1.0f);
				m_axis.Normalize();
				Vec2 normal = Utilities.Mul(xfB.q, m_axis);

				m_localPoint = 0.5f * (localPointB1 + localPointB2);
				Vec2 pointB = Utilities.Mul(xfB, m_localPoint);

				Vec2 localPointA = proxyA.GetVertex(cache.indexA[0]);
				Vec2 pointA = Utilities.Mul(xfA, localPointA);

				float s = Utilities.Dot(pointA - pointB, normal);
				if (s < 0.0f)
				{
					m_axis = -m_axis;
					s = -s;
				}
				return s;
			}
			else
			{
				// Two points on A and one or two points on B.
				m_type = SeparationType.e_faceA;
				Vec2 localPointA1 = m_proxyA.GetVertex(cache.indexA[0]);
				Vec2 localPointA2 = m_proxyA.GetVertex(cache.indexA[1]);
			
				m_axis = Utilities.Cross(localPointA2 - localPointA1, 1.0f);
				m_axis.Normalize();
				Vec2 normal = Utilities.Mul(xfA.q, m_axis);

				m_localPoint = 0.5f * (localPointA1 + localPointA2);
				Vec2 pointA = Utilities.Mul(xfA, m_localPoint);

				Vec2 localPointB = m_proxyB.GetVertex(cache.indexB[0]);
				Vec2 pointB = Utilities.Mul(xfB, localPointB);

				float s = Utilities.Dot(pointB - pointA, normal);
				if (s < 0.0f)
				{
					m_axis = -m_axis;
					s = -s;
				}
				return s;
			}
		}