コード例 #1
0
ファイル: Manifold.cs プロジェクト: debuggerpls/pongDOTS
        private static unsafe void ConvexConvex(
            Context context, ColliderKeyPair colliderKeys,
            Collider *convexColliderA, Collider *convexColliderB, MTransform worldFromA, MTransform worldFromB,
            float maxDistance, bool flipped)
        {
            Material materialA = ((ConvexColliderHeader *)convexColliderA)->Material;
            Material materialB = ((ConvexColliderHeader *)convexColliderB)->Material;

            // Skip if the bodies have infinite mass and the materials don't want to raise any solver events,
            // since the resulting contacts can't have any effect during solving
            if (context.BodiesHaveInfiniteMass)
            {
                if (((materialA.Flags | materialB.Flags) & (Material.MaterialFlags.IsTrigger | Material.MaterialFlags.EnableCollisionEvents)) == 0)
                {
                    return;
                }
            }

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

            WriteManifold(contactManifold, context, colliderKeys, materialA, materialB, flipped);
        }
コード例 #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]);
                }
            }
        }