Exemple #1
0
        public static unsafe Result ConvexConvex(
            float3 *verticesA, int numVerticesA, float convexRadiusA,
            float3 *verticesB, int numVerticesB, float convexRadiusB,
            MTransform aFromB)
        {
            ConvexConvexDistanceQueries.Result result = ConvexConvexDistanceQueries.ConvexConvex(
                verticesA, numVerticesA, verticesB, numVerticesB, aFromB, ConvexConvexDistanceQueries.PenetrationHandling.Exact3D);

            // Adjust for convex radii
            result.ClosestPoints.Distance       -= (convexRadiusA + convexRadiusB);
            result.ClosestPoints.PositionOnAinA -= result.ClosestPoints.NormalInA * convexRadiusA;
            return(result.ClosestPoints);
        }
        // Create contact points for a pair of generic convex hulls in world space.
        public static unsafe void ConvexConvex(
            ref ConvexHull hullA, ref ConvexHull hullB,
            MTransform worldFromA, MTransform aFromB, float maxDistance,
            out Manifold manifold)
        {
            // Get closest points on the hulls
            ConvexConvexDistanceQueries.Result result = ConvexConvexDistanceQueries.ConvexConvex(
                hullA.VerticesPtr, hullA.NumVertices, hullB.VerticesPtr, hullB.NumVertices, aFromB, ConvexConvexDistanceQueries.PenetrationHandling.Exact3D);

            float sumRadii = hullB.ConvexRadius + hullA.ConvexRadius;

            if (result.ClosestPoints.Distance < maxDistance + sumRadii)
            {
                float3 normal = result.ClosestPoints.NormalInA;

                manifold = new Manifold
                {
                    Normal = math.mul(worldFromA.Rotation, normal)
                };

                if (hullA.NumFaces > 0)
                {
                    int faceIndexA = hullA.GetSupportingFace(-normal, result.SimplexVertexA(0));
                    if (hullB.NumFaces > 0)
                    {
                        // Convex vs convex
                        int faceIndexB = hullB.GetSupportingFace(math.mul(math.transpose(aFromB.Rotation), normal), result.SimplexVertexB(0));
                        if (FaceFace(ref hullA, ref hullB, faceIndexA, faceIndexB, worldFromA, aFromB, normal, result.ClosestPoints.Distance, ref manifold))
                        {
                            return;
                        }
                    }
                    else if (hullB.NumVertices == 2)
                    {
                        // Convex vs capsule
                        if (FaceEdge(ref hullA, ref hullB, faceIndexA, worldFromA, aFromB, normal, result.ClosestPoints.Distance, ref manifold))
                        {
                            return;
                        }
                    } // Else convex vs sphere
                }
                else if (hullA.NumVertices == 2)
                {
                    if (hullB.NumFaces > 0)
                    {
                        // Capsule vs convex
                        manifold.Normal = math.mul(worldFromA.Rotation, -normal); // negate the normal because we are temporarily flipping to triangle A capsule B
                        MTransform worldFromB        = Mul(worldFromA, aFromB);
                        MTransform bFromA            = Inverse(aFromB);
                        float3     normalInB         = math.mul(bFromA.Rotation, normal);
                        int        faceIndexB        = hullB.GetSupportingFace(normalInB, result.SimplexVertexB(0));
                        bool       foundClosestPoint = FaceEdge(ref hullB, ref hullA, faceIndexB, worldFromB, bFromA, -normalInB, result.ClosestPoints.Distance, ref manifold);
                        manifold.Flip();
                        if (foundClosestPoint)
                        {
                            return;
                        }
                    } // Else capsule vs capsule or sphere
                }     // Else sphere vs something

                // Either one of the shapes is a sphere, or both of the shapes are capsules, or both of the closest features are nearly perpendicular to the contact normal,
                // or FaceFace()/FaceEdge() missed the closest point due to numerical error.  In these cases, add the closest point directly to the manifold.
                if (manifold.NumContacts < Manifold.k_MaxNumContacts)
                {
                    DistanceQueries.Result convexDistance = result.ClosestPoints;
                    manifold[manifold.NumContacts++] = new ContactPoint
                    {
                        Position = Mul(worldFromA, convexDistance.PositionOnAinA) - manifold.Normal * (convexDistance.Distance - hullB.ConvexRadius),
                        Distance = convexDistance.Distance - sumRadii
                    };
                }
            }
            else
            {
                manifold = new Manifold();
            }
        }