Exemplo n.º 1
0
        /// <summary>
        ///     Reads the cache using the specified cache
        /// </summary>
        /// <param name="cache">The cache</param>
        /// <param name="shapeA">The shape</param>
        /// <param name="transformA">The transform</param>
        /// <param name="shapeB">The shape</param>
        /// <param name="transformB">The transform</param>
        internal unsafe void ReadCache(SimplexCache *cache, Shape shapeA, XForm transformA, Shape shapeB,
                                       XForm transformB)
        {
            Box2DxDebug.Assert(0 <= cache->Count && cache->Count <= 3);

            // Copy data from cache.
            Count = cache->Count;
            SimplexVertex **vertices = stackalloc SimplexVertex *[3];

            fixed(SimplexVertex *v1Ptr = &V1, v2Ptr = &V2, v3Ptr = &V3)
            {
                vertices[0] = v1Ptr;
                vertices[1] = v2Ptr;
                vertices[2] = v3Ptr;
                for (int i = 0; i < Count; ++i)
                {
                    SimplexVertex *v = vertices[i];
                    v->IndexA = cache->IndexA[i];
                    v->IndexB = cache->IndexB[i];
                    Vec2 wALocal = shapeA.GetVertex(v->IndexA);
                    Vec2 wBLocal = shapeB.GetVertex(v->IndexB);
                    v->Wa = Math.Mul(transformA, wALocal);
                    v->Wb = Math.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 (Count > 1)
                {
                    float metric1 = cache->Metric;
                    float metric2 = GetMetric();
                    if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Settings.FltEpsilon)
                    {
                        // Reset the simplex.
                        Count = 0;
                    }
                }

                // If the cache is empty or invalid ...
                if (Count == 0)
                {
                    SimplexVertex *v = vertices[0];
                    v->IndexA = 0;
                    v->IndexB = 0;
                    Vec2 wALocal = shapeA.GetVertex(0);
                    Vec2 wBLocal = shapeB.GetVertex(0);
                    v->Wa = Math.Mul(transformA, wALocal);
                    v->Wb = Math.Mul(transformB, wBLocal);
                    v->W  = v->Wb - v->Wa;
                    Count = 1;
                }
            }
        }
Exemplo n.º 2
0
        internal unsafe void ReadCache(SimplexCache *cache, Shape shapeA, Transform TransformA, Shape shapeB, Transform TransformB)
        {
            Box2DXDebug.Assert(0 <= cache->Count && cache->Count <= 3);

            // Copy data from cache.
            _count = cache->Count;
            SimplexVertex **vertices = stackalloc SimplexVertex *[3];

            fixed(SimplexVertex *v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3)
            {
                vertices[0] = v1Ptr;
                vertices[1] = v2Ptr;
                vertices[2] = v3Ptr;
                for (int i = 0; i < _count; ++i)
                {
                    SimplexVertex *v = vertices[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 = TransformA.TransformPoint(wALocal);
                    v->wB = TransformB.TransformPoint(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 (_count > 1)
                {
                    float metric1 = cache->Metric;
                    float metric2 = GetMetric();
                    if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < Common.Settings.FLT_EPSILON)
                    {
                        // Reset the simplex.
                        _count = 0;
                    }
                }

                // If the cache is empty or invalid ...
                if (_count == 0)
                {
                    SimplexVertex *v = vertices[0];
                    v->indexA = 0;
                    v->indexB = 0;
                    Vector2 wALocal = shapeA.GetVertex(0);
                    Vector2 wBLocal = shapeB.GetVertex(0);
                    v->wA  = TransformA.TransformPoint(wALocal);
                    v->wB  = TransformB.TransformPoint(wBLocal);
                    v->w   = v->wB - v->wA;
                    _count = 1;
                }
            }
        }
Exemplo n.º 3
0
        internal unsafe void WriteCache(SimplexCache *cache)
        {
            cache->Metric = GetMetric();
            cache->Count  = (UInt16)_count;
            SimplexVertex **vertices = stackalloc SimplexVertex *[3];

            fixed(SimplexVertex *v1Ptr = &_v1, v2Ptr = &_v2, v3Ptr = &_v3)
            {
                vertices[0] = v1Ptr;
                vertices[1] = v2Ptr;
                vertices[2] = v3Ptr;
                for (int i = 0; i < _count; ++i)
                {
                    cache->IndexA[i] = (Byte)(vertices[i]->indexA);
                    cache->IndexB[i] = (Byte)(vertices[i]->indexB);
                }
            }
        }
Exemplo n.º 4
0
        /// <summary>
        ///     Writes the cache using the specified cache
        /// </summary>
        /// <param name="cache">The cache</param>
        internal unsafe void WriteCache(SimplexCache *cache)
        {
            cache->Metric = GetMetric();
            cache->Count  = (ushort)Count;
            SimplexVertex **vertices = stackalloc SimplexVertex *[3];

            fixed(SimplexVertex *v1Ptr = &V1, v2Ptr = &V2, v3Ptr = &V3)
            {
                vertices[0] = v1Ptr;
                vertices[1] = v2Ptr;
                vertices[2] = v3Ptr;
                for (int i = 0; i < Count; ++i)
                {
                    cache->IndexA[i] = (byte)vertices[i]->IndexA;
                    cache->IndexB[i] = (byte)vertices[i]->IndexB;
                }
            }
        }
Exemplo n.º 5
0
        internal unsafe void Initialize(SimplexCache *cache,
                                        Shape shapeA, XForm transformA,
                                        Shape shapeB, XForm transformB)
        {
            ShapeA = shapeA;
            ShapeB = shapeB;
            int count = cache->Count;

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

            if (count == 1)
            {
                FaceType = Type.Points;
                Vec2 localPointA = ShapeA.GetVertex(cache->IndexA[0]);
                Vec2 localPointB = ShapeB.GetVertex(cache->IndexB[0]);
                Vec2 pointA      = Common.Math.Mul(transformA, localPointA);
                Vec2 pointB      = Common.Math.Mul(transformB, localPointB);
                Axis = pointB - pointA;
                Axis.Normalize();
            }
            else if (cache->IndexB[0] == cache->IndexB[1])
            {
                // Two points on A and one on B
                FaceType = Type.FaceA;
                Vec2 localPointA1 = ShapeA.GetVertex(cache->IndexA[0]);
                Vec2 localPointA2 = ShapeA.GetVertex(cache->IndexA[1]);
                Vec2 localPointB  = ShapeB.GetVertex(cache->IndexB[0]);
                LocalPoint = 0.5f * (localPointA1 + localPointA2);
                Axis       = Vec2.Cross(localPointA2 - localPointA1, 1.0f);
                Axis.Normalize();

                Vec2 normal = Common.Math.Mul(transformA.R, Axis);
                Vec2 pointA = Common.Math.Mul(transformA, LocalPoint);
                Vec2 pointB = Common.Math.Mul(transformB, localPointB);

                float s = Vec2.Dot(pointB - pointA, normal);
                if (s < 0.0f)
                {
                    Axis = -Axis;
                }
            }
            else
            {
                // Two points on B and one or two points on A.
                // We ignore the second point on A.
                FaceType = Type.FaceB;
                Vec2 localPointA  = shapeA.GetVertex(cache->IndexA[0]);
                Vec2 localPointB1 = shapeB.GetVertex(cache->IndexB[0]);
                Vec2 localPointB2 = shapeB.GetVertex(cache->IndexB[1]);
                LocalPoint = 0.5f * (localPointB1 + localPointB2);
                Axis       = Vec2.Cross(localPointB2 - localPointB1, 1.0f);
                Axis.Normalize();

                Vec2 normal = Common.Math.Mul(transformB.R, Axis);
                Vec2 pointB = Common.Math.Mul(transformB, LocalPoint);
                Vec2 pointA = Common.Math.Mul(transformA, localPointA);

                float s = Vec2.Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    Axis = -Axis;
                }
            }
        }
        internal unsafe void Initialize(SimplexCache *cache,
                                        Shape shapeA, Transform TransformA,
                                        Shape shapeB, Transform TransformB)
        {
            ShapeA = shapeA;
            ShapeB = shapeB;
            int count = cache->Count;

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

            if (count == 1)
            {
                FaceType = Type.Points;
                Vector2 localPointA = ShapeA.GetVertex(cache->IndexA[0]);
                Vector2 localPointB = ShapeB.GetVertex(cache->IndexB[0]);
                Vector2 pointA      = TransformA.TransformPoint(localPointA);
                Vector2 pointB      = TransformB.TransformPoint(localPointB);
                Axis = pointB - pointA;
                Axis.Normalize();
            }
            else if (cache->IndexB[0] == cache->IndexB[1])
            {
                // Two points on A and one on B
                FaceType = Type.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       = (localPointA2 - localPointA1).CrossScalarPostMultiply(1.0f);
                Axis.Normalize();

                Vector2 normal = TransformA.TransformDirection(Axis);
                Vector2 pointA = TransformA.TransformPoint(LocalPoint);
                Vector2 pointB = TransformB.TransformPoint(localPointB);

                float s = Vector2.Dot(pointB - pointA, normal);
                if (s < 0.0f)
                {
                    Axis = -Axis;
                }
            }
            else
            {
                // Two points on B and one or two points on A.
                // We ignore the second point on A.
                FaceType = Type.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       = (localPointB2 - localPointB1).CrossScalarPostMultiply(1.0f);
                Axis.Normalize();

                Vector2 normal = TransformB.TransformDirection(Axis);
                Vector2 pointB = TransformB.TransformPoint(LocalPoint);
                Vector2 pointA = TransformA.TransformPoint(localPointA);

                float s = Vector2.Dot(pointA - pointB, normal);
                if (s < 0.0f)
                {
                    Axis = -Axis;
                }
            }
        }