Ejemplo n.º 1
0
        private static unsafe void WriteManifold(ConvexConvexManifoldQueries.Manifold manifold, Context context, ColliderKeyPair colliderKeys,
                                                 Material materialA, Material materialB, bool flipped)
        {
            // Write results to stream
            if (manifold.NumContacts > 0)
            {
                if (flipped)
                {
                    manifold.Flip();
                }

                var header = new ContactHeader
                {
                    BodyPair       = context.BodyIndices,
                    BodyCustomTags = context.BodyCustomTags,
                    NumContacts    = manifold.NumContacts,
                    Normal         = manifold.Normal,
                    ColliderKeys   = colliderKeys
                };

                // Apply materials
                {
                    Material.MaterialFlags combinedFlags = materialA.Flags | materialB.Flags;
                    if ((combinedFlags & Material.MaterialFlags.IsTrigger) != 0)
                    {
                        header.JacobianFlags |= JacobianFlags.IsTrigger;
                    }
                    else
                    {
                        if ((combinedFlags & Material.MaterialFlags.EnableCollisionEvents) != 0)
                        {
                            header.JacobianFlags |= JacobianFlags.EnableCollisionEvents;
                        }
                        if ((combinedFlags & Material.MaterialFlags.EnableMassFactors) != 0)
                        {
                            header.JacobianFlags |= JacobianFlags.EnableMassFactors;
                        }
                        if ((combinedFlags & Material.MaterialFlags.EnableSurfaceVelocity) != 0)
                        {
                            header.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity;
                        }

                        header.CoefficientOfFriction    = Material.GetCombinedFriction(materialA, materialB);
                        header.CoefficientOfRestitution = Material.GetCombinedRestitution(materialA, materialB);
                    }
                }

                context.ContactWriter->Write(header);

                // Group the contact points in 2s (when 4-6 contact points) and 3s (6 or more contact points)
                // to avoid the order forcing the magnitude of the impulse on one side of the face.
                // When less than 4 contact points access them in order.
                int startIndex = 0;
                int increment  = header.NumContacts < 6 ?
                                 math.max(header.NumContacts / 2, 1) : (header.NumContacts / 3 + ((header.NumContacts % 3 > 0) ? 1 : 0));
                for (int contactIndex = 0; ; contactIndex += increment)
                {
                    if (contactIndex >= header.NumContacts)
                    {
                        startIndex++;
                        if (startIndex == increment)
                        {
                            break;
                        }
                        contactIndex = startIndex;
                    }

                    context.ContactWriter->Write(manifold[contactIndex]);
                }
            }
        }
Ejemplo n.º 2
0
        private static unsafe void ConvexConvex(
            Context context, ColliderKeyPair colliderKeys,
            Collider *convexColliderA, Collider *convexColliderB, MTransform worldFromA, MTransform worldFromB,
            float maxDistance, bool flipped, ref BlockStream.Writer contactWriter)
        {
            MTransform aFromB = Mul(Inverse(worldFromA), worldFromB);

            ConvexConvexManifoldQueries.Manifold contactManifold;
            switch (convexColliderA->Type)
            {
            case ColliderType.Sphere:
                switch (convexColliderB->Type)
                {
                case ColliderType.Sphere:
                    ConvexConvexManifoldQueries.SphereSphere(
                        (SphereCollider *)convexColliderA, (SphereCollider *)convexColliderB,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                case ColliderType.Capsule:
                    ConvexConvexManifoldQueries.CapsuleSphere(
                        (CapsuleCollider *)convexColliderB, (SphereCollider *)convexColliderA,
                        worldFromB, Inverse(aFromB), maxDistance, out contactManifold);
                    flipped = !flipped;
                    break;

                case ColliderType.Triangle:
                    ConvexConvexManifoldQueries.TriangleSphere(
                        (PolygonCollider *)convexColliderB, (SphereCollider *)convexColliderA,
                        worldFromB, Inverse(aFromB), maxDistance, out contactManifold);
                    flipped = !flipped;
                    break;

                case ColliderType.Box:
                    ConvexConvexManifoldQueries.BoxSphere(
                        (BoxCollider *)convexColliderB, (SphereCollider *)convexColliderA,
                        worldFromB, Inverse(aFromB), maxDistance, out contactManifold);
                    flipped = !flipped;
                    break;

                case ColliderType.Quad:
                case ColliderType.Cylinder:
                case ColliderType.Convex:
                    ConvexConvexManifoldQueries.ConvexConvex(
                        ref ((SphereCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                default:
                    throw new NotImplementedException();
                }
                break;

            case ColliderType.Box:
                switch (convexColliderB->Type)
                {
                case ColliderType.Sphere:
                    ConvexConvexManifoldQueries.BoxSphere(
                        (BoxCollider *)convexColliderA, (SphereCollider *)convexColliderB,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                case ColliderType.Triangle:
                    ConvexConvexManifoldQueries.BoxTriangle(
                        (BoxCollider *)convexColliderA, (PolygonCollider *)convexColliderB,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                case ColliderType.Box:
                    ConvexConvexManifoldQueries.BoxBox(
                        (BoxCollider *)convexColliderA, (BoxCollider *)convexColliderB,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                case ColliderType.Capsule:
                case ColliderType.Quad:
                case ColliderType.Cylinder:
                case ColliderType.Convex:
                    ConvexConvexManifoldQueries.ConvexConvex(
                        ref ((BoxCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                default:
                    throw new NotImplementedException();
                }
                break;

            case ColliderType.Capsule:
                switch (convexColliderB->Type)
                {
                case ColliderType.Sphere:
                    ConvexConvexManifoldQueries.CapsuleSphere(
                        (CapsuleCollider *)convexColliderA, (SphereCollider *)convexColliderB,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                case ColliderType.Capsule:
                    ConvexConvexManifoldQueries.CapsuleCapsule(
                        (CapsuleCollider *)convexColliderA, (CapsuleCollider *)convexColliderB,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                case ColliderType.Triangle:
                    ConvexConvexManifoldQueries.CapsuleTriangle(
                        (CapsuleCollider *)convexColliderA, (PolygonCollider *)convexColliderB,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                case ColliderType.Quad:
                case ColliderType.Box:
                case ColliderType.Cylinder:
                case ColliderType.Convex:
                    ConvexConvexManifoldQueries.ConvexConvex(
                        ref ((CapsuleCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                default:
                    throw new NotImplementedException();
                }
                break;

            case ColliderType.Triangle:
                switch (convexColliderB->Type)
                {
                case ColliderType.Sphere:
                    ConvexConvexManifoldQueries.TriangleSphere(
                        (PolygonCollider *)convexColliderA, (SphereCollider *)convexColliderB,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                case ColliderType.Capsule:
                    ConvexConvexManifoldQueries.CapsuleTriangle(
                        (CapsuleCollider *)convexColliderB, (PolygonCollider *)convexColliderA,
                        worldFromB, Inverse(aFromB), maxDistance, out contactManifold);
                    flipped = !flipped;
                    break;

                case ColliderType.Box:
                    ConvexConvexManifoldQueries.BoxTriangle(
                        (BoxCollider *)convexColliderB, (PolygonCollider *)convexColliderA,
                        worldFromB, Inverse(aFromB), maxDistance, out contactManifold);
                    flipped = !flipped;
                    break;

                case ColliderType.Triangle:
                case ColliderType.Quad:
                case ColliderType.Cylinder:
                case ColliderType.Convex:
                    ConvexConvexManifoldQueries.ConvexConvex(
                        ref ((PolygonCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull,
                        worldFromA, aFromB, maxDistance, out contactManifold);
                    break;

                default:
                    throw new NotImplementedException();
                }
                break;

            case ColliderType.Quad:
            case ColliderType.Cylinder:
            case ColliderType.Convex:
                ConvexConvexManifoldQueries.ConvexConvex(
                    ref ((ConvexCollider *)convexColliderA)->ConvexHull, ref ((ConvexCollider *)convexColliderB)->ConvexHull,
                    worldFromA, aFromB, maxDistance, out contactManifold);
                break;

            default:
                throw new NotImplementedException();
            }

            // Write results to stream
            if (contactManifold.NumContacts > 0)
            {
                if (flipped)
                {
                    contactManifold.Flip();
                }

                var header = new ContactHeader
                {
                    BodyPair        = context.BodyIndices,
                    BodyCustomDatas = context.BodyCustomDatas,
                    NumContacts     = contactManifold.NumContacts,
                    Normal          = contactManifold.Normal,
                    ColliderKeys    = colliderKeys
                };

                // Apply materials
                {
                    Material materialA = ((ConvexColliderHeader *)convexColliderA)->Material;
                    Material materialB = ((ConvexColliderHeader *)convexColliderB)->Material;
                    Material.MaterialFlags combinedFlags = materialA.Flags | materialB.Flags;
                    if ((combinedFlags & Material.MaterialFlags.IsTrigger) != 0)
                    {
                        header.JacobianFlags |= JacobianFlags.IsTrigger;
                    }
                    else
                    {
                        if ((combinedFlags & Material.MaterialFlags.EnableCollisionEvents) != 0)
                        {
                            header.JacobianFlags |= JacobianFlags.EnableCollisionEvents;
                        }
                        if ((combinedFlags & Material.MaterialFlags.EnableMassFactors) != 0)
                        {
                            header.JacobianFlags |= JacobianFlags.EnableMassFactors;
                        }
                        if ((combinedFlags & Material.MaterialFlags.EnableSurfaceVelocity) != 0)
                        {
                            header.JacobianFlags |= JacobianFlags.EnableSurfaceVelocity;
                        }
                        if ((combinedFlags & Material.MaterialFlags.EnableMaxImpulse) != 0)
                        {
                            header.JacobianFlags |= JacobianFlags.EnableMaxImpulse;
                        }

                        header.CoefficientOfFriction    = Material.GetCombinedFriction(materialA, materialB);
                        header.CoefficientOfRestitution = Material.GetCombinedRestitution(materialA, materialB);
                    }
                }

                contactWriter.Write(header);

                for (int contactIndex = 0; contactIndex < header.NumContacts; contactIndex++)
                {
                    contactWriter.Write(contactManifold[contactIndex]);
                }
            }
        }