public float ComputeTOI(Sweep sweepA, Sweep sweepB)
        {
            TOIInput input = new TOIInput();
            input.SweepA = sweepA;
            input.SweepB = sweepB;
            input.SweepRadiusA = _fixtureA.ComputeSweepRadius(sweepA.LocalCenter);
            input.SweepRadiusB = _fixtureB.ComputeSweepRadius(sweepB.LocalCenter);
            input.Tolerance = Common.Settings.LinearSlop;

            return Collision.Collision.TimeOfImpact(input, _fixtureA.Shape, _fixtureB.Shape);
        }
Example #2
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;

            Box2DXDebug.Assert(sweepA.T0 == sweepB.T0);
            Box2DXDebug.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();

            cache.Count = 0;
            DistanceInput distanceInput;

            distanceInput.UseRadii = false;

            for (; ;)
            {
                Transform 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();
#if ALLOWUNSAFE
                unsafe
                {
                    fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB);
                }
#else
                fcn.Initialize(cache, shapeA, xfA, shapeB, xfB);
#endif

                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.
                    if (separation > radius)
                    {
                        target = Common.Math.Max(radius - tolerance, 0.75f * radius);
                    }
                    else
                    {
                        target = 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];
        /// <summary>
        /// CCD via the secant method.
        /// 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.
        /// fraction=0 means the shapes begin touching/overlapped, and fraction=1 means the shapes don't touch.
        /// </summary>
        /// <param name="input">The input.</param>
        /// <param name="shapeA">The shape A.</param>
        /// <param name="shapeB">The shape B.</param>
        /// <returns>
        /// fraction between [0,1] in which the shapes first touch.
        /// </returns>
        public static float TimeOfImpact(TOIInput input)
        {
            ++ToiCalls;

            DistanceProxy proxyA = input.ProxyA;
            DistanceProxy proxyB = input.ProxyB;

            Sweep sweepA = input.SweepA;
            Sweep sweepB = input.SweepB;

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

            float radius = proxyA._radius + proxyB._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();
            cache.Count = 0;
            DistanceInput distanceInput = new DistanceInput();
            distanceInput.proxyA = input.ProxyA;
            distanceInput.proxyB = input.ProxyB;
            distanceInput.UseRadii = false;

            for (;;)
            {
                Transform 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, cache, distanceInput);

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

                SeparationFunction fcn = new SeparationFunction();
                fcn.Initialize(cache, proxyA, xfA, proxyB, 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.
                    if (separation > radius)
                    {
                        target = Math.Max(radius - tolerance, 0.75f*radius);
                    }
                    else
                    {
                        target = 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 int N = 100;
            float dx = 1.0f / N;
            float xs[N+1];