示例#1
0
        //
        // Reference implementations of queries using simple brute-force methods
        //

        static unsafe float RefConvexConvexDistance(ref ConvexHull a, ref ConvexHull b, MTransform aFromB)
        {
            bool success = false;

            if (a.NumVertices + b.NumVertices < 64) // too slow without burst
            {
                // Build the minkowski difference in a-space
                int  maxNumVertices = a.NumVertices * b.NumVertices;
                Aabb aabb           = Aabb.Empty;
                for (int iB = 0; iB < b.NumVertices; iB++)
                {
                    float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
                    for (int iA = 0; iA < a.NumVertices; iA++)
                    {
                        float3 vertexA = a.Vertices[iA];
                        aabb.Include(vertexA - vertexB);
                    }
                }
                ConvexHullBuilderStorage diffStorage = new ConvexHullBuilderStorage(maxNumVertices, Allocator.Temp, aabb, 0.0f, ConvexHullBuilder.IntResolution.Low);
                ref ConvexHullBuilder    diff        = ref diffStorage.Builder;
                success = true;
                for (int iB = 0; iB < b.NumVertices; iB++)
                {
                    float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
                    for (int iA = 0; iA < a.NumVertices; iA++)
                    {
                        float3 vertexA = a.Vertices[iA];
                        diff.AddPoint(vertexA - vertexB, (uint)(iA | iB << 16));
                    }
                }

                float distance = 0.0f;
                if (success && diff.Dimension == 3)
                {
                    // Find the closest triangle to the origin
                    distance = float.MaxValue;
                    bool penetrating = true;
                    foreach (int t in diff.Triangles.Indices)
                    {
                        ConvexHullBuilder.Triangle triangle = diff.Triangles[t];
                        float3 v0 = diff.Vertices[triangle.GetVertex(0)].Position;
                        float3 v1 = diff.Vertices[triangle.GetVertex(1)].Position;
                        float3 v2 = diff.Vertices[triangle.GetVertex(2)].Position;
                        float3 n  = diff.ComputePlane(t).Normal;
                        DistanceQueries.Result result = DistanceQueries.TriangleSphere(v0, v1, v2, n, float3.zero, 0.0f, MTransform.Identity);
                        if (result.Distance < distance)
                        {
                            distance = result.Distance;
                        }
                        penetrating = penetrating & (math.dot(n, -result.NormalInA) < 0.0f); // only penetrating if inside of all planes
                    }

                    if (penetrating)
                    {
                        distance = -distance;
                    }

                    distance -= a.ConvexRadius + b.ConvexRadius;
                }
                else
                {
                    success = false;
                }

                diffStorage.Dispose();

                if (success)
                {
                    return(distance);
                }
            }
示例#2
0
        //
        // Reference implementations of queries using simple brute-force methods
        //

        static unsafe float RefConvexConvexDistance(ref ConvexHull a, ref ConvexHull b, MTransform aFromB)
        {
            // Build the minkowski difference in a-space
            int maxNumVertices     = a.NumVertices * b.NumVertices;
            ConvexHullBuilder diff = new ConvexHullBuilder(maxNumVertices, 2 * maxNumVertices, Allocator.Temp);
            bool success           = true;
            Aabb aabb = Aabb.Empty;

            for (int iB = 0; iB < b.NumVertices; iB++)
            {
                float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
                for (int iA = 0; iA < a.NumVertices; iA++)
                {
                    float3 vertexA = a.Vertices[iA];
                    aabb.Include(vertexA - vertexB);
                }
            }
            diff.IntegerSpaceAabb = aabb;
            for (int iB = 0; iB < b.NumVertices; iB++)
            {
                float3 vertexB = Math.Mul(aFromB, b.Vertices[iB]);
                for (int iA = 0; iA < a.NumVertices; iA++)
                {
                    float3 vertexA = a.Vertices[iA];
                    if (!diff.AddPoint(vertexA - vertexB, (uint)(iA | iB << 16)))
                    {
                        // TODO - coplanar vertices are tripping up ConvexHullBuilder, we should fix it but for now fall back to DistanceQueries.ConvexConvex()
                        success = false;
                    }
                }
            }

            float distance;

            if (!success || diff.Triangles.GetFirstIndex() == -1)
            {
                // No triangles unless the difference is 3D, fall back to GJK
                // Most of the time this happens for cases like sphere-sphere, capsule-capsule, etc. which have special implementations,
                // so comparing those to GJK still validates the results of different API queries against each other.
                distance = DistanceQueries.ConvexConvex(ref a, ref b, aFromB).Distance;
            }
            else
            {
                // Find the closest triangle to the origin
                distance = float.MaxValue;
                bool penetrating = true;
                for (int t = diff.Triangles.GetFirstIndex(); t != -1; t = diff.Triangles.GetNextIndex(t))
                {
                    ConvexHullBuilder.Triangle triangle = diff.Triangles[t];
                    float3 v0 = diff.Vertices[triangle.GetVertex(0)].Position;
                    float3 v1 = diff.Vertices[triangle.GetVertex(1)].Position;
                    float3 v2 = diff.Vertices[triangle.GetVertex(2)].Position;
                    float3 n  = diff.ComputePlane(t).Normal;
                    DistanceQueries.Result result = DistanceQueries.TriangleSphere(v0, v1, v2, n, float3.zero, 0.0f, MTransform.Identity);
                    if (result.Distance < distance)
                    {
                        distance = result.Distance;
                    }
                    penetrating = penetrating & (math.dot(n, -result.NormalInA) < 0.0f); // only penetrating if inside of all planes
                }

                if (penetrating)
                {
                    distance = -distance;
                }

                distance -= a.ConvexRadius + b.ConvexRadius;
            }

            diff.Dispose();
            return(distance);
        }