예제 #1
0
        /// 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.
        /// @return 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.
        public static float CalculateTimeOfImpact(ref TOIInput input, Shape shapeA, Shape shapeB)
        {
            ++b2_toiCalls;

            Sweep sweepA = input.sweepA;
            Sweep sweepB = input.sweepB;

            Debug.Assert(sweepA.t0 == sweepB.t0);
            Debug.Assert(1.0f - sweepA.t0 > Settings.b2_FLT_EPSILON);

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

            float alpha = 0.0f;

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

            // Prepare input for distance query.
            SimplexCache cache;
            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.ComputeDistance(out distanceOutput, out cache, ref distanceInput, shapeA, shapeB);

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

                SeparationFunction fcn = new SeparationFunction(ref cache, shapeA, ref xfA, shapeB, ref xfB);

                float separation = fcn.Evaluate(ref xfA, ref 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
                {
                    int N = 100;
                    float dx = 1.0f / N;
                    float xs[N+1];
예제 #2
0
        /// 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.
        /// @return 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.
        public static float CalculateTimeOfImpact(ref TOIInput input, Shape shapeA, Shape shapeB)
        {
            ++b2_toiCalls;

            Sweep sweepA = input.sweepA;
            Sweep sweepB = input.sweepB;

            Debug.Assert(sweepA.t0 == sweepB.t0);
            Debug.Assert(1.0f - sweepA.t0 > Settings.b2_FLT_EPSILON);

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

            float alpha = 0.0f;

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

            // Prepare input for distance query.
            SimplexCache  cache;
            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.ComputeDistance(out distanceOutput, out cache, ref distanceInput, shapeA, shapeB);

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

                SeparationFunction fcn = new SeparationFunction(ref cache, shapeA, ref xfA, shapeB, ref xfB);

                float separation = fcn.Evaluate(ref xfA, ref 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
                {
                    int   N  = 100;
                    float dx = 1.0f / N;
                    float xs[N + 1];