Example #1
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;
            }
        }
        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;
            }
        }
Example #3
0
        public SeparationFunction(ref SimplexCache cache,
                                  ref DistanceProxy proxyA, ref Sweep sweepA,
                                  ref DistanceProxy proxyB, ref Sweep sweepB)
        {
            _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, 0.0f);
            _sweepB.GetTransform(out xfB, 0.0f);

            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;
            }
        }
        public SeparationFunction(ref SimplexCache cache,
            ref DistanceProxy proxyA, ref Sweep sweepA,
            ref DistanceProxy proxyB, ref Sweep sweepB)
        {
            _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, 0.0f);
            _sweepB.GetTransform(out xfB, 0.0f);

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