GetVertex() public method

Get a vertex by index. Used by b2Distance.
public GetVertex ( int index ) : Vector2
index int The index.
return Vector2
        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.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;
            }
        }
Ejemplo n.º 2
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]);

                Vector2 a = localPointB2 - localPointB1;
                _axis = new Vector2(a.Y, -a.X);
                _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]);

                Vector2 a = localPointA2 - localPointA1;
                _axis = new Vector2(a.Y, -a.X);
                _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;
            }
        }
Ejemplo n.º 3
0
        public float FindMinSeparation(out int indexA, out int indexB, float t)
        {
            Transform xfA, xfB;

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

            switch (_type)
            {
            case SeparationFunctionType.Points:
            {
                Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, _axis);
                Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -_axis);

                indexA = _proxyA.GetSupport(axisA);
                indexB = _proxyB.GetSupport(axisB);

                Vector2 localPointA = _proxyA.GetVertex(indexA);
                Vector2 localPointB = _proxyB.GetVertex(indexB);

                Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA);
                Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB);

                float separation = Vector2.Dot(pointB - pointA, _axis);
                return(separation);
            }

            case SeparationFunctionType.FaceA:
            {
                Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis);
                Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint);

                Vector2 axisB = MathUtils.MultiplyT(ref xfB.R, -normal);

                indexA = -1;
                indexB = _proxyB.GetSupport(axisB);

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

                float separation = Vector2.Dot(pointB - pointA, normal);
                return(separation);
            }

            case SeparationFunctionType.FaceB:
            {
                Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis);
                Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint);

                Vector2 axisA = MathUtils.MultiplyT(ref xfA.R, -normal);

                indexB = -1;
                indexA = _proxyA.GetSupport(axisA);

                Vector2 localPointA = _proxyA.GetVertex(indexA);
                Vector2 pointA      = MathUtils.Multiply(ref xfA, localPointA);

                float separation = Vector2.Dot(pointA - pointB, normal);
                return(separation);
            }

            default:
                Debug.Assert(false);
                indexA = -1;
                indexB = -1;
                return(0.0f);
            }
        }
        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]);

                Vector2 a = localPointB2 - localPointB1;
                _axis = new Vector2(a.Y, -a.X);
                _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]);

                Vector2 a = localPointA2 - localPointA1;
                _axis = new Vector2(a.Y, -a.X);
                _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;
            }
        }
        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.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;
            }
        }