internal void Initialize(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
        {
            ShapeA = shapeA;
            ShapeB = shapeB;
            int count = cache.Count;

            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;
                }
            }
        }
Example #2
0
 internal void WriteCache(SimplexCache cache)
 {
     cache.Metric = GetMetric();
     cache.Count  = (UInt16)_count;
     SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 };
     for (int i = 0; i < _count; ++i)
     {
         cache.IndexA[i] = (Byte)(vertices[i].indexA);
         cache.IndexB[i] = (Byte)(vertices[i].indexB);
     }
 }
        internal void ReadCache(SimplexCache cache, Shape shapeA, Transform transformA, Shape shapeB, Transform transformB)
        {
            Box2DNetDebug.Assert(0 <= cache.Count && cache.Count <= 3);

            // Copy data from cache.
            _count = cache.Count;
            SimplexVertex[] vertices = new SimplexVertex[] { _v1, _v2, _v3 };
            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;
            }
        }
Example #4
0
            void ReadCache(SimplexCache cache, DistanceProxy proxyA, XForm transformA, DistanceProxy proxyB, XForm transformB)
            {
                // Copy data from cache.
                m_count = cache.count;
                SimplexVertex[] vertices = m_v1;
                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 = MathB2.Mul(transformA, wALocal);
                    v.wB = MathB2.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 < Settings.FLT_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    = MathB2.Mul(transformA, wALocal);
                    v.wB    = MathB2.Mul(transformB, wBLocal);
                    v.w     = v.wB - v.wA;
                    v.a     = 1.0f;
                    m_count = 1;
                }
            }
Example #5
0
        static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB)
        {
            output = new DistanceOutput();

            Transform transformA = input.TransformA;
            Transform transformB = input.TransformB;

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

#if ALLOWUNSAFE
            fixed(SimplexCache *sPtr = &cache)
            {
                simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB);
            }
#else
            simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB);
#endif

            // Get simplex vertices as an array.
#if ALLOWUNSAFE
            SimplexVertex *vertices = &simplex._v1;
#else
            SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 };
#endif

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
#if ALLOWUNSAFE
            int *lastA = stackalloc int[4], lastB = stackalloc int[4];
#else
            int[] lastA = new int[4];
            int[] lastB = new int[4];
#endif // ALLOWUNSAFE
            int lastCount;

            // Main iteration loop.
            int       iter = 0;
            const int k_maxIterationCount = 20;
            while (iter < k_maxIterationCount)
            {
                // Copy simplex so we can identify duplicates.
                lastCount = simplex._count;
                int i;
                for (i = 0; i < lastCount; ++i)
                {
                    lastA[i] = vertices[i].indexA;
                    lastB[i] = vertices[i].indexB;
                }

                switch (simplex._count)
                {
                case 1:
                    break;

                case 2:
                    simplex.Solve2();
                    break;

                case 3:
                    simplex.Solve3();
                    break;

                default:
                    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();
                float   distanceSqr = p.LengthSquared();

                // Ensure the search direction is numerically fit.
                if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED)
                {
                    // 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.
#if ALLOWUNSAFE
                SimplexVertex *vertex = vertices + simplex._count;
                vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
                vertex->wA     = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA));
                //Vec2 wBLocal;
                vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
                vertex->wB     = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB));
                vertex->w      = vertex->wB - vertex->wA;
#else
                SimplexVertex vertex = vertices[simplex._count - 1];
                vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p));
                vertex.wA     = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA));
                //Vec2 wBLocal;
                vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p));
                vertex.wB     = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB));
                vertex.w      = vertex.wB - vertex.wA;
#endif // ALLOWUNSAFE

                // Iteration count is equated to the number of support point calls.
                ++iter;

                // Check for convergence.
#if ALLOWUNSAFE
                float lowerBound = Vector2.Dot(p, vertex->w);
#else
                float lowerBound = Vector2.Dot(p, vertex.w);
#endif
                float       upperBound       = distanceSqr;
                const float k_relativeTolSqr = 0.01f * 0.01f;                   // 1:100
                if (upperBound - lowerBound <= k_relativeTolSqr * upperBound)
                {
                    // Converged!
                    break;
                }

                // Check for duplicate support points.
                bool duplicate = false;
                for (i = 0; i < lastCount; ++i)
                {
#if ALLOWUNSAFE
                    if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i])
#else
                    if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i])
#endif
                    {
                        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;
            }


#if ALLOWUNSAFE
            fixed(DistanceOutput *doPtr = &output)
            {
                // Prepare output.
                simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB);
                doPtr->Distance   = Vector2.Distance(doPtr->PointA, doPtr->PointB);
                doPtr->Iterations = iter;
            }

            fixed(SimplexCache *sPtr = &cache)
            {
                // Cache the simplex.
                simplex.WriteCache(sPtr);
            }
#else
            // Prepare output.
            simplex.GetWitnessPoints(ref output.PointA, ref output.PointB);
            output.Distance   = Box2DNet.Common.Math.Distance(output.PointA, output.PointB);
            output.Iterations = iter;

            // Cache the simplex.
            simplex.WriteCache(cache);
#endif

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

                if (output.Distance > rA + rB && output.Distance > Common.Settings.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 #6
0
        // CCD via the secant method.
        /// <summary>
        /// Compute the time when two shapes begin to touch or touch at a closer distance.
        /// TOI considers the shape radii. It attempts to have the radii overlap by the tolerance.
        /// Iterations terminate with the overlap is within 0.5 * tolerance. The tolerance should be
        /// smaller than sum of the shape radii.
        /// Warning the sweeps must have the same time interval.
        /// </summary>
        /// <returns>
        /// The fraction between [0,1] in which the shapes first touch.
        /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
        /// </returns>
        public static float TimeOfImpact(TOIInput input, Shape shapeA, Shape shapeB)
        {
            Sweep sweepA = input.SweepA;
            Sweep sweepB = input.SweepB;

            Box2DNetDebug.Assert(sweepA.T0 == sweepB.T0);
            Box2DNetDebug.Assert(1.0f - sweepA.T0 > Common.Settings.FLT_EPSILON);

            float radius    = shapeA._radius + shapeB._radius;
            float tolerance = input.Tolerance;

            float alpha = 0.0f;

            const int k_maxIterations = 1000;                   // TODO_ERIN b2Settings
            int       iter            = 0;
            float     target          = 0.0f;

            // Prepare input for distance query.
            SimplexCache cache = new SimplexCache {
                Count = 0
            };
            DistanceInput distanceInput;

            distanceInput.UseRadii = false;

            for (; ;)
            {
                XForm xfA, xfB;
                sweepA.GetTransform(out xfA, alpha);
                sweepB.GetTransform(out xfB, alpha);

                // Get the distance between shapes.
                distanceInput.TransformA = xfA;
                distanceInput.TransformB = xfB;
                DistanceOutput distanceOutput;
                Distance(out distanceOutput, ref cache, ref distanceInput, shapeA, shapeB);

                if (distanceOutput.Distance <= 0.0f)
                {
                    alpha = 1.0f;
                    break;
                }

                SeparationFunction fcn = new SeparationFunction();
                unsafe
                {
                    fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB);
                }

                float separation = fcn.Evaluate(xfA, xfB);
                if (separation <= 0.0f)
                {
                    alpha = 1.0f;
                    break;
                }

                if (iter == 0)
                {
                    // Compute a reasonable target distance to give some breathing room
                    // for conservative advancement. We take advantage of the shape radii
                    // to create additional clearance.
                    target = separation > radius?Common.Math.Max(radius - tolerance, 0.75f *radius) : Common.Math.Max(separation - tolerance, 0.02f * radius);
                }

                if (separation - target < 0.5f * tolerance)
                {
                    if (iter == 0)
                    {
                        alpha = 1.0f;
                        break;
                    }

                    break;
                }

#if _FALSE
                // Dump the curve seen by the root finder
                {
                    const int32 N  = 100;
                    float32     dx = 1.0f / N;
                    float32     xs[N + 1];