public static void ComputeDistance(out DistanceOutput output,
            out SimplexCache cache,
            ref DistanceInput input)
        {
            cache = new SimplexCache();
            ++b2_gjkCalls;

            // Initialize the simplex.
            Simplex simplex = new Simplex();
            simplex.ReadCache(ref cache, ref input.proxyA, ref input.transformA, ref input.proxyB, ref input.transformB);

            // Get simplex vertices as an array.
            const int k_maxIters = 20;

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
            FixedArray3<int> saveA = new FixedArray3<int>();
            FixedArray3<int> saveB = new FixedArray3<int>();
            int saveCount = 0;

            Vector2 closestPoint = simplex.GetClosestPoint();
            float distanceSqr1 = closestPoint.LengthSquared();
            float distanceSqr2 = distanceSqr1;

            // Main iteration loop.
            int iter = 0;
            while (iter < k_maxIters)
            {
                // Copy simplex so we can identify duplicates.
                saveCount = simplex._count;
                for (int i = 0; i < saveCount; ++i)
                {
                    saveA[i] = simplex._v[i].indexA;
                    saveB[i] = simplex._v[i].indexB;
                }

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

                    case 2:
                        simplex.Solve2();
                        break;

                    case 3:
                        simplex.Solve3();
                        break;

                    default:
                        Debug.Assert(false);
                        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();
                distanceSqr2 = p.LengthSquared();

                // Ensure progress
                if (distanceSqr2 >= distanceSqr1)
                {
                    //break;
                }
                distanceSqr1 = distanceSqr2;

                // Get search direction.
                Vector2 d = simplex.GetSearchDirection();

                // Ensure the search direction is numerically fit.
                if (d.LengthSquared() < Settings.b2_epsilon * Settings.b2_epsilon)
                {
                    // 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.
                SimplexVertex vertex = simplex._v[simplex._count];
                vertex.indexA = input.proxyA.GetSupport(MathUtils.MultiplyT(ref input.transformA.R, -d));
                vertex.wA = MathUtils.Multiply(ref input.transformA, input.proxyA.GetVertex(vertex.indexA));

                vertex.indexB = input.proxyB.GetSupport(MathUtils.MultiplyT(ref input.transformB.R, d));
                vertex.wB = MathUtils.Multiply(ref input.transformB, input.proxyB.GetVertex(vertex.indexB));
                vertex.w = vertex.wB - vertex.wA;
                simplex._v[simplex._count] = vertex;

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

                // Check for duplicate support points. This is the main termination criteria.
                bool duplicate = false;
                for (int i = 0; i < saveCount; ++i)
                {
                    if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i])
                    {
                        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;
            }

            b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter);

            // Prepare output.
            simplex.GetWitnessPoints(out output.pointA, out output.pointB);
            output.distance = (output.pointA - output.pointB).Length();
            output.iterations = iter;

            // Cache the simplex.
            simplex.WriteCache(ref cache);

            // Apply radii if requested.
            if (input.useRadii)
            {
                float rA = input.proxyA._radius;
                float rB = input.proxyB._radius;

                if (output.distance > rA + rB && output.distance > Settings.b2_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;
                }
            }
        }
Exemple #2
0
        public static void ComputeDistance(out DistanceOutput output,
                                           out SimplexCache cache,
                                           ref DistanceInput input)
        {
            cache = new SimplexCache();
            ++b2_gjkCalls;

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

            simplex.ReadCache(ref cache, ref input.proxyA, ref input.transformA, ref input.proxyB, ref input.transformB);

            // Get simplex vertices as an array.
            int k_maxIters = 20;

            // These store the vertices of the last simplex so that we
            // can check for duplicates and prevent cycling.
            FixedArray3 <int> saveA = new FixedArray3 <int>();
            FixedArray3 <int> saveB = new FixedArray3 <int>();
            int saveCount           = 0;

            Vector2 closestPoint = simplex.GetClosestPoint();
            float   distanceSqr1 = closestPoint.LengthSquared();
            float   distanceSqr2 = distanceSqr1;

            // Main iteration loop.
            int iter = 0;

            while (iter < k_maxIters)
            {
                // Copy simplex so we can identify duplicates.
                saveCount = simplex._count;
                for (int i = 0; i < saveCount; ++i)
                {
                    saveA[i] = simplex._v[i].indexA;
                    saveB[i] = simplex._v[i].indexB;
                }

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

                case 2:
                    simplex.Solve2();
                    break;

                case 3:
                    simplex.Solve3();
                    break;

                default:
                    Debug.Assert(false);
                    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();
                distanceSqr2 = p.LengthSquared();

                // Ensure progress
                if (distanceSqr2 >= distanceSqr1)
                {
                    //break;
                }
                distanceSqr1 = distanceSqr2;

                // Get search direction.
                Vector2 d = simplex.GetSearchDirection();

                // Ensure the search direction is numerically fit.
                if (d.LengthSquared() < Settings.b2_epsilon * Settings.b2_epsilon)
                {
                    // 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.
                SimplexVertex vertex = simplex._v[simplex._count];
                vertex.indexA = input.proxyA.GetSupport(MathUtils.MultiplyT(ref input.transformA.R, -d));
                vertex.wA     = MathUtils.Multiply(ref input.transformA, input.proxyA.GetVertex(vertex.indexA));

                vertex.indexB = input.proxyB.GetSupport(MathUtils.MultiplyT(ref input.transformB.R, d));
                vertex.wB     = MathUtils.Multiply(ref input.transformB, input.proxyB.GetVertex(vertex.indexB));
                vertex.w      = vertex.wB - vertex.wA;
                simplex._v[simplex._count] = vertex;

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

                // Check for duplicate support points. This is the main termination criteria.
                bool duplicate = false;
                for (int i = 0; i < saveCount; ++i)
                {
                    if (vertex.indexA == saveA[i] && vertex.indexB == saveB[i])
                    {
                        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;
            }

            b2_gjkMaxIters = Math.Max(b2_gjkMaxIters, iter);

            // Prepare output.
            simplex.GetWitnessPoints(out output.pointA, out output.pointB);
            output.distance   = (output.pointA - output.pointB).Length();
            output.iterations = iter;

            // Cache the simplex.
            simplex.WriteCache(ref cache);

            // Apply radii if requested.
            if (input.useRadii)
            {
                float rA = input.proxyA._radius;
                float rB = input.proxyB._radius;

                if (output.distance > rA + rB && output.distance > Settings.b2_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;
                }
            }
        }