예제 #1
0
        public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
        {
            cache = default(SimplexCache);
            Simplex simplex = default(Simplex);

            simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);
            FixedArray3 <int> fixedArray  = default(FixedArray3 <int>);
            FixedArray3 <int> fixedArray2 = default(FixedArray3 <int>);
            int i = 0;

            while (i < 20)
            {
                int count = simplex.Count;
                for (int j = 0; j < count; j++)
                {
                    fixedArray[j]  = simplex.V[j].IndexA;
                    fixedArray2[j] = simplex.V[j].IndexB;
                }
                switch (simplex.Count)
                {
                case 1:
                    break;

                case 2:
                    simplex.Solve2();
                    break;

                case 3:
                    simplex.Solve3();
                    break;

                default:
                    Debug.Assert(false);
                    break;
                }
                bool flag = simplex.Count == 3;
                if (flag)
                {
                    break;
                }
                TSVector2 searchDirection = simplex.GetSearchDirection();
                bool      flag2           = searchDirection.LengthSquared() < Settings.EpsilonSqr;
                if (flag2)
                {
                    break;
                }
                SimplexVertex simplexVertex = simplex.V[simplex.Count];
                simplexVertex.IndexA     = input.ProxyA.GetSupport(MathUtils.MulT(input.TransformA.q, -searchDirection));
                simplexVertex.WA         = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[simplexVertex.IndexA]);
                simplexVertex.IndexB     = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, searchDirection));
                simplexVertex.WB         = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[simplexVertex.IndexB]);
                simplexVertex.W          = simplexVertex.WB - simplexVertex.WA;
                simplex.V[simplex.Count] = simplexVertex;
                i++;
                bool flag3 = false;
                for (int k = 0; k < count; k++)
                {
                    bool flag4 = simplexVertex.IndexA == fixedArray[k] && simplexVertex.IndexB == fixedArray2[k];
                    if (flag4)
                    {
                        flag3 = true;
                        break;
                    }
                }
                bool flag5 = flag3;
                if (flag5)
                {
                    break;
                }
                simplex.Count++;
            }
            simplex.GetWitnessPoints(out output.PointA, out output.PointB);
            output.Distance   = (output.PointA - output.PointB).magnitude;
            output.Iterations = i;
            simplex.WriteCache(ref cache);
            bool useRadii = input.UseRadii;

            if (useRadii)
            {
                FP   radius  = input.ProxyA.Radius;
                FP   radius2 = input.ProxyB.Radius;
                bool flag6   = output.Distance > radius + radius2 && output.Distance > Settings.Epsilon;
                if (flag6)
                {
                    output.Distance -= radius + radius2;
                    TSVector2 value = output.PointB - output.PointA;
                    value.Normalize();
                    output.PointA += radius * value;
                    output.PointB -= radius2 * value;
                }
                else
                {
                    TSVector2 tSVector = 0.5f * (output.PointA + output.PointB);
                    output.PointA   = tSVector;
                    output.PointB   = tSVector;
                    output.Distance = 0f;
                }
            }
        }
예제 #2
0
        public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input)
        {
            cache = new SimplexCache();

            if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
            {
                ++GJKCalls;
            }

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

            simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB);

            // 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>();

            //FP distanceSqr1 = Settings.MaxFP;

            // Main iteration loop.
            int iter = 0;

            while (iter < Settings.MaxGJKIterations)
            {
                // Copy simplex so we can identify duplicates.
                int 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;
                }

                //FPE: This code was not used anyway.
                // Compute closest point.
                //Vector2 p = simplex.GetClosestPoint();
                //FP distanceSqr2 = p.LengthSquared();

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

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

                // Ensure the search direction is numerically fit.
                if (d.LengthSquared() < Settings.EpsilonSqr)
                {
                    // 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.MulT(input.TransformA.q, -d));
                vertex.WA     = MathUtils.Mul(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]);

                vertex.IndexB            = input.ProxyB.GetSupport(MathUtils.MulT(input.TransformB.q, d));
                vertex.WB                = MathUtils.Mul(ref input.TransformB, input.ProxyB.Vertices[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;

                if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
                {
                    ++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;
            }

            if (Settings.EnableDiagnostics) //FPE: We only gather diagnostics when enabled
            {
                GJKMaxIters = Math.Max(GJKMaxIters, iter);
            }

            // Prepare output.
            simplex.GetWitnessPoints(out output.PointA, out output.PointB);
            output.Distance   = (output.PointA - output.PointB).magnitude;
            output.Iterations = iter;

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

            // Apply radii if requested.
            if (input.UseRadii)
            {
                FP rA = input.ProxyA.Radius;
                FP rB = input.ProxyB.Radius;

                if (output.Distance > rA + rB && output.Distance > Settings.Epsilon)
                {
                    // Shapes are still no overlapped.
                    // Move the witness points to the outer surface.
                    output.Distance -= rA + rB;
                    TSVector2 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.
                    TSVector2 p = 0.5f * (output.PointA + output.PointB);
                    output.PointA   = p;
                    output.PointB   = p;
                    output.Distance = 0.0f;
                }
            }
        }