Exemplo n.º 1
0
        // Create contact points for a capsule and triangle in world space.
        public static unsafe void CapsuleTriangle(
            CapsuleCollider *capsuleA, PolygonCollider *triangleB,
            MTransform worldFromA, MTransform aFromB, float maxDistance,
            out Manifold manifold)
        {
            Assert.IsTrue(triangleB->Vertices.Length == 3);

            DistanceQueries.Result convexDistance = DistanceQueries.CapsuleTriangle(capsuleA, triangleB, aFromB);
            if (convexDistance.Distance < maxDistance)
            {
                // Build manifold
                manifold = new Manifold
                {
                    Normal = math.mul(worldFromA.Rotation, -convexDistance.NormalInA) // 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, convexDistance.NormalInA);
                int        faceIndexB = triangleB->ConvexHull.GetSupportingFace(normalInB);
                if (FaceEdge(ref triangleB->ConvexHull, ref capsuleA->ConvexHull, faceIndexB, worldFromB, bFromA, -normalInB, convexDistance.Distance + capsuleA->Radius, ref manifold))
                {
                    manifold.Flip();
                }
                else
                {
                    manifold = new Manifold(convexDistance, worldFromA);
                }
            }
            else
            {
                manifold = new Manifold();
            }
        }
Exemplo n.º 2
0
 // Create a single point manifold from a distance query result
 public Manifold(DistanceQueries.Result convexDistance, MTransform worldFromA)
 {
     NumContacts = 1;
     Normal      = math.mul(worldFromA.Rotation, convexDistance.NormalInA);
     this[0]     = new ContactPoint
     {
         Distance = convexDistance.Distance,
         Position = Mul(worldFromA, convexDistance.PositionOnBinA)
     };
 }
Exemplo n.º 3
0
 // Create a contact point for a pair of spheres in world space.
 public static unsafe void SphereSphere(
     SphereCollider *sphereA, SphereCollider *sphereB,
     MTransform worldFromA, MTransform aFromB, float maxDistance,
     out Manifold manifold)
 {
     DistanceQueries.Result convexDistance = DistanceQueries.SphereSphere(sphereA, sphereB, aFromB);
     if (convexDistance.Distance < maxDistance)
     {
         manifold = new Manifold(convexDistance, worldFromA);
     }
     else
     {
         manifold = new Manifold();
     }
 }
Exemplo n.º 4
0
 // Create a contact point for a pair of capsules in world space.
 public static unsafe void CapsuleCapsule(
     CapsuleCollider *capsuleA, CapsuleCollider *capsuleB,
     MTransform worldFromA, MTransform aFromB, float maxDistance,
     out Manifold manifold)
 {
     // TODO: Should produce a multi-point manifold
     DistanceQueries.Result convexDistance = DistanceQueries.CapsuleCapsule(capsuleA, capsuleB, aFromB);
     if (convexDistance.Distance < maxDistance)
     {
         manifold = new Manifold(convexDistance, worldFromA);
     }
     else
     {
         manifold = new Manifold();
     }
 }
Exemplo n.º 5
0
 // Create a single point manifold between a capsule and a sphere in world space.
 public static unsafe void CapsuleSphere(
     CapsuleCollider *capsuleA, SphereCollider *sphereB,
     MTransform worldFromA, MTransform aFromB, float maxDistance,
     out Manifold manifold)
 {
     DistanceQueries.Result convexDistance = DistanceQueries.CapsuleSphere(
         capsuleA->Vertex0, capsuleA->Vertex1, capsuleA->Radius, sphereB->Center, sphereB->Radius, aFromB);
     if (convexDistance.Distance < maxDistance)
     {
         manifold = new Manifold(convexDistance, worldFromA);
     }
     else
     {
         manifold = new Manifold();
     }
 }
Exemplo n.º 6
0
        // Create a single point manifold between a triangle and sphere in world space.
        public static unsafe void TriangleSphere(
            PolygonCollider *triangleA, SphereCollider *sphereB,
            MTransform worldFromA, MTransform aFromB, float maxDistance,
            out Manifold manifold)
        {
            Assert.IsTrue(triangleA->Vertices.Length == 3);

            DistanceQueries.Result convexDistance = DistanceQueries.TriangleSphere(
                triangleA->Vertices[0], triangleA->Vertices[1], triangleA->Vertices[2], triangleA->Planes[0].Normal,
                sphereB->Center, sphereB->Radius, aFromB);
            if (convexDistance.Distance < maxDistance)
            {
                manifold = new Manifold(convexDistance, worldFromA);
            }
            else
            {
                manifold = new Manifold();
            }
        }
Exemplo n.º 7
0
        // 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();
            }
        }