Exemplo n.º 1
0
        public static unsafe void AabbCollider <T>(OverlapAabbInput input, [NoAlias] Collider *collider, [NoAlias] ref T collector)
            where T : struct, IOverlapCollector
        {
            if (!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter))
            {
                return;
            }

            switch (collider->Type)
            {
            case ColliderType.Mesh:
                AabbMesh(input, (MeshCollider *)collider, ref collector);
                break;

            case ColliderType.Compound:
                AabbCompound(input, (CompoundCollider *)collider, ref collector);
                break;

            case ColliderType.Terrain:
                AabbTerrain(input, (TerrainCollider *)collider, ref collector);
                break;

            default:
                SafetyChecks.ThrowNotImplementedException();
                return;
            }
        }
Exemplo n.º 2
0
        private static unsafe void CompositeComposite(
            Context context,
            Collider *compositeColliderA, Collider *compositeColliderB, MTransform worldFromA, MTransform worldFromB,
            MotionExpansion expansion, bool flipped)
        {
            // Flip the order if necessary, so that A has fewer leaves than B
            if (compositeColliderA->NumColliderKeyBits > compositeColliderB->NumColliderKeyBits)
            {
                Collider *c = compositeColliderA;
                compositeColliderA = compositeColliderB;
                compositeColliderB = c;

                MTransform t = worldFromA;
                worldFromA = worldFromB;
                worldFromB = t;

                expansion.Linear *= -1.0f;
                flipped           = !flipped;
            }

            var collector = new CompositeCompositeLeafCollector(
                context,
                compositeColliderA, compositeColliderB,
                worldFromA, worldFromB, expansion, flipped);

            compositeColliderA->GetLeaves(ref collector);
        }
Exemplo n.º 3
0
        private static unsafe void DrawConvex(
            Collider *collider,
            ref PhysicsTransform worldTransform,
            ref PhysicsDebugStreamSystem.Context outputStream,
            Color colliderColor)
        {
            var convexCollider = (ConvexCollider *)collider;
            var vertexCount    = convexCollider->VertexCount;
            var vertices       = convexCollider->Vertices;
            var convexRadius   = convexCollider->m_ConvexHull.ConvexRadius;

            switch (collider->ColliderType)
            {
            case ColliderType.Box:
            case ColliderType.Polygon:
            {
                Assert.IsTrue(vertexCount >= 3, "ConvexCollider must have >= 3 vertices.");

                outputStream.Polygon(vertices, vertexCount, worldTransform, colliderColor);
                return;
            }

            case ColliderType.Circle:
            {
                Assert.AreEqual(1, vertexCount, "CircleCollider must have 1 vertex.");

                var position = PhysicsMath.mul(worldTransform, vertices[0]);
                outputStream.Circle(position, convexRadius, colliderColor);
                return;
            }

            case ColliderType.Capsule:
            {
                Assert.AreEqual(2, vertexCount, "CapsuleCollider must have 2 vertices.");

                var vertex0 = PhysicsMath.mul(worldTransform, vertices[0]);
                var vertex1 = PhysicsMath.mul(worldTransform, vertices[1]);
                var offset  = PhysicsMath.perp(math.normalizesafe(vertex1 - vertex0)) * new float2(convexRadius);

                // Side Edges.
                {
                    outputStream.Line(vertex0 - offset, vertex1 - offset, colliderColor);
                    outputStream.Line(vertex0 + offset, vertex1 + offset, colliderColor);
                }

                // End Caps.
                {
                    var startAngle = math.atan2(offset.y, offset.x);
                    var endAngle   = startAngle + math.PI;
                    outputStream.Arc(vertex0, convexRadius, startAngle, endAngle, colliderColor);
                    outputStream.Arc(vertex1, convexRadius, startAngle + math.PI, endAngle + math.PI, colliderColor);
                }

                return;
            }

            default:
                return;
            }
        }
Exemplo n.º 4
0
        static void PushoutFromCapsule(Collider *pCollider, ColliderEx *pColliderEx, ref Vector3 point)
        {
            var capsuleVec    = pColliderEx->Direction;
            var capsulePos    = pColliderEx->Position;
            var targetVec     = point - capsulePos;
            var radius        = pCollider->Radius;
            var distanceOnVec = Vector3.Dot(capsuleVec, targetVec);

            if (distanceOnVec <= 0.0f)
            {
                PushoutFromSphere(capsulePos, radius, ref point);
            }
            else if (distanceOnVec >= pCollider->Height)
            {
                PushoutFromSphere(capsulePos + capsuleVec * distanceOnVec, radius, ref point);
            }
            else
            {
                var positionOnVec   = capsulePos + (capsuleVec * distanceOnVec);
                var pushoutVec      = point - positionOnVec;
                var distanceSquared = pushoutVec.sqrMagnitude;
                if (distanceSquared > Epsilon && distanceSquared < radius * radius)
                {
                    var distance = Mathf.Sqrt(distanceSquared);
                    point = positionOnVec + pushoutVec * radius / distance;
                }
            }
        }
        public unsafe void Execute <TCtx>(TCtx ctx, InputTriggerPort port) where TCtx : IGraphInstance
        {
            var entity = ctx.ReadEntity(Entity);

            if (entity == Unity.Entities.Entity.Null)
            {
                return;
            }

            if (!ctx.EntityManager.HasComponent <PhysicsCollider>(entity))
            {
                return;
            }

            PhysicsCollider physicsCollider = ctx.EntityManager.GetComponentData <PhysicsCollider>(entity);
            Collider *      colliderPtr     = (Collider *)physicsCollider.Value.GetUnsafePtr();

            CollisionFilter newFilter = new CollisionFilter();

            newFilter.BelongsTo    = (uint)ctx.ReadInt(this.BelongsTo);
            newFilter.CollidesWith = (uint)ctx.ReadInt(this.CollidesWith);
            newFilter.GroupIndex   = ctx.ReadInt(this.GroupIndex);

            colliderPtr->Filter = newFilter;

            ctx.Trigger(Output);
        }
Exemplo n.º 6
0
        void IJobParallelFor.Execute(int index)
        {
            var pRW = pRWPoints + index;

            if (IsEnableFloor)
            {
                if (pRW->Position.y <= FloorHeight)
                {
                    pRW->Position.y = FloorHeight;
                }
            }

            if (IsEnableCollider)
            {
                for (int i = 0; i < ColliderCount; ++i)
                {
                    Collider *  pCollider   = pColliders + i;
                    ColliderEx *pColliderEx = pColliderExs + i;

                    if (pCollider->Height <= 0.0f)
                    {
                        PushoutFromSphere(pCollider, pColliderEx, ref pRW->Position);
                    }
                    else
                    {
                        PushoutFromCapsule(pCollider, pColliderEx, ref pRW->Position);
                    }
                }
            }
        }
Exemplo n.º 7
0
        private static unsafe void ConvexComposite(
            Context context, ColliderKey convexKeyA,
            Collider *convexColliderA, Collider *compositeColliderB, MTransform worldFromA, MTransform worldFromB,
            MotionExpansion expansion, bool flipped, ref BlockStream.Writer contactWriter)
        {
            // Calculate AABB of A in B
            MTransform bFromWorld = Inverse(worldFromB);
            MTransform bFromA     = Mul(bFromWorld, worldFromA);
            var        transform  = new RigidTransform(new quaternion(bFromA.Rotation), bFromA.Translation); // TODO: avoid this conversion to and back from float3x3
            Aabb       aabbAinB   = expansion.ExpandAabb(convexColliderA->CalculateAabb(transform));

            // Do the midphase query and build manifolds for any overlapping leaf colliders
            var input = new OverlapAabbInput {
                Aabb = aabbAinB, Filter = convexColliderA->Filter
            };
            var collector = new ConvexCompositeOverlapCollector(
                context,
                convexColliderA, convexKeyA, compositeColliderB,
                worldFromA, worldFromB, expansion.MaxDistance, flipped,
                contactWriter);

            OverlapQueries.AabbCollider(input, compositeColliderB, ref collector);

            // Keep updated writer state
            contactWriter = collector.m_ContactWriter;
        }
Exemplo n.º 8
0
    //
    // Object creation
    //

    private unsafe Entity CreateBody(float3 position, quaternion orientation, BlobAssetReference <Collider> collider,
                                     float3 linearVelocity, float3 angularVelocity, float mass, bool isDynamic)
    {
        EntityManager entityManager = EntityManager;

        Entity entity = entityManager.CreateEntity(new ComponentType[] { });

        entityManager.AddComponentData(entity, new LocalToWorld {
        });
        entityManager.AddComponentData(entity, new Translation {
            Value = position
        });
        entityManager.AddComponentData(entity, new Rotation {
            Value = orientation
        });
        entityManager.AddComponentData(entity, new PhysicsCollider {
            Value = collider
        });

        List <Unity.Physics.Authoring.DisplayBodyColliders.DrawComponent.DisplayResult> meshes = Unity.Physics.Authoring.DisplayBodyColliders.DrawComponent.BuildDebugDisplayMesh((Collider *)collider.GetUnsafePtr());

        CombineInstance[] instances = new CombineInstance[meshes.Count];
        for (int i = 0; i < meshes.Count; i++)
        {
            instances[i] = new CombineInstance
            {
                mesh      = meshes[i].Mesh,
                transform = Matrix4x4.TRS(meshes[i].Position, meshes[i].Orientation, meshes[i].Scale)
            };
        }
        Mesh mesh = new Mesh();

        mesh.CombineMeshes(instances);

        entityManager.AddSharedComponentData(entity, new RenderMesh
        {
            mesh     = mesh,
            material = isDynamic ? dynamicMaterial : staticMaterial
        });

        if (isDynamic)
        {
            Collider *colliderPtr = (Collider *)collider.GetUnsafePtr();
            entityManager.AddComponentData(entity, PhysicsMass.CreateDynamic(colliderPtr->MassProperties, mass));

            float3 angularVelocityLocal = math.mul(math.inverse(colliderPtr->MassProperties.MassDistribution.Transform.rot), angularVelocity);
            entityManager.AddComponentData(entity, new PhysicsVelocity()
            {
                Linear  = linearVelocity,
                Angular = angularVelocityLocal
            });
            entityManager.AddComponentData(entity, new PhysicsDamping()
            {
                Linear  = 0.01f,
                Angular = 0.05f
            });
        }

        return(entity);
    }
Exemplo n.º 9
0
        void PushoutFromCapsule(Collider *pCollider, ColliderEx *pColliderEx, ref Vector3 point)
        {
            var capsuleVec       = pColliderEx->Direction;
            var capsuleVecNormal = capsuleVec.normalized;
            var capsulePos       = pColliderEx->Position;
            var targetVec        = point - capsulePos;
            var distanceOnVec    = Vector3.Dot(capsuleVecNormal, targetVec);

            if (distanceOnVec <= EPSILON)
            {
                PushoutFromSphere(capsulePos, pCollider->RadiusHead, ref point);
                return;
            }
            else if (distanceOnVec >= pCollider->Height)
            {
                PushoutFromSphere(capsulePos + capsuleVec, pCollider->RadiusTail, ref point);
                return;
            }
            else
            {
                var positionOnVec      = capsulePos + (capsuleVecNormal * distanceOnVec);
                var pushoutVec         = point - positionOnVec;
                var sqrPushoutDistance = pushoutVec.sqrMagnitude;
                if (sqrPushoutDistance > EPSILON)
                {
                    var Radius = pCollider->RadiusHead + (pCollider->RadiusTail - pCollider->RadiusHead) * distanceOnVec;
                    if (sqrPushoutDistance < Radius * Radius)
                    {
                        var pushoutDistance = Mathf.Sqrt(sqrPushoutDistance);
                        point = positionOnVec + pushoutVec * Radius / pushoutDistance;
                        return;
                    }
                }
            }
        }
Exemplo n.º 10
0
        private static unsafe bool ConvexConvex(ColliderCastInput input, Collider *target, sfloat maxFraction, out ColliderCastHit hit)
        {
            hit = default;

            // Get the current transform
            MTransform targetFromQuery = new MTransform(input.Orientation, input.Start);

            // Conservative advancement
            sfloat tolerance    = sfloat.FromRaw(0x3a83126f); // return if this close to a hit
            sfloat keepDistance = sfloat.FromRaw(0x38d1b717); // avoid bad cases for GJK (penetration / exact hit)
            int    iterations   = 10;                         // return after this many advances, regardless of accuracy
            sfloat fraction     = sfloat.Zero;

            while (true)
            {
                if (fraction >= maxFraction)
                {
                    // Exceeded the maximum fraction without a hit
                    return(false);
                }

                // Find the current distance
                DistanceQueries.Result distanceResult = DistanceQueries.ConvexConvex(target, input.Collider, targetFromQuery);

                // Check for a hit
                if (distanceResult.Distance < tolerance || --iterations == 0)
                {
                    targetFromQuery.Translation = input.Start;
                    hit.Position       = Mul(input.QueryContext.WorldFromLocalTransform, distanceResult.PositionOnBinA);
                    hit.SurfaceNormal  = math.mul(input.QueryContext.WorldFromLocalTransform.Rotation, -distanceResult.NormalInA);
                    hit.Fraction       = fraction;
                    hit.RigidBodyIndex = input.QueryContext.RigidBodyIndex;
                    hit.ColliderKey    = input.QueryContext.ColliderKey;
                    hit.Material       = ((ConvexColliderHeader *)target)->Material;
                    hit.Entity         = input.QueryContext.Entity;

                    return(true);
                }

                // Check for a miss
                sfloat dot = math.dot(distanceResult.NormalInA, input.Ray.Displacement);
                if (dot <= sfloat.Zero)
                {
                    // Collider is moving away from the target, it will never hit
                    return(false);
                }

                // Advance
                fraction += (distanceResult.Distance - keepDistance) / dot;
                if (fraction >= maxFraction)
                {
                    // Exceeded the maximum fraction without a hit
                    return(false);
                }

                targetFromQuery.Translation = math.lerp(input.Start, input.End, fraction);
            }
        }
Exemplo n.º 11
0
        public unsafe void Execute <TCtx>(TCtx ctx, InputTriggerPort port) where TCtx : IGraphInstance
        {
            var entity = ctx.ReadEntity(Entity);

            if (entity == Unity.Entities.Entity.Null)
            {
                return;
            }

            if (!ctx.EntityManager.HasComponent <PhysicsCollider>(entity))
            {
                return;
            }

            PhysicsCollider physicsCollider = ctx.EntityManager.GetComponentData <PhysicsCollider>(entity);
            Collider *      colliderPtr     = (Collider *)physicsCollider.Value.GetUnsafePtr();

            if (!ctx.EntityManager.HasComponent <PhysicsGravityFactor>(entity))
            {
                ctx.EntityManager.AddComponent <PhysicsGravityFactor>(entity);
            }
            float gravityFactor = ctx.ReadFloat(GravityFactor);

            ctx.EntityManager.SetComponentData(entity, new PhysicsGravityFactor()
            {
                Value = gravityFactor
            });

            if (!ctx.EntityManager.HasComponent <PhysicsMass>(entity))
            {
                ctx.EntityManager.AddComponent <PhysicsMass>(entity);
            }
            float mass = ctx.ReadFloat(Mass);

            ctx.EntityManager.SetComponentData(entity, PhysicsMass.CreateDynamic(colliderPtr->MassProperties, mass));

            if (!ctx.EntityManager.HasComponent <PhysicsVelocity>(entity))
            {
                ctx.EntityManager.AddComponent <PhysicsVelocity>(entity);
            }
            ctx.EntityManager.SetComponentData(entity, new PhysicsVelocity {
                Linear = float3.zero, Angular = float3.zero
            });

            float drag        = ctx.ReadFloat(Drag);
            float angularDrag = ctx.ReadFloat(AngularDrag);

            if (!ctx.EntityManager.HasComponent <PhysicsDamping>(entity))
            {
                ctx.EntityManager.AddComponent <PhysicsDamping>(entity);
            }
            ctx.EntityManager.SetComponentData(entity, new PhysicsDamping {
                Linear = drag, Angular = angularDrag
            });

            ctx.Trigger(Output);
        }
Exemplo n.º 12
0
        private static unsafe bool ConvexConvex <T>(ColliderCastInput input, Collider *target, ref T collector) where T : struct, ICollector <ColliderCastHit>
        {
            //Assert.IsTrue(target->CollisionType == CollisionType.Convex && input.Collider->CollisionType == CollisionType.Convex, "ColliderCast.ConvexConvex can only process convex colliders");

            // Get the current transform
            MTransform targetFromQuery = new MTransform(input.Orientation, input.Start);

            // Conservative advancement
            const float tolerance    = 1e-3f;   // return if this close to a hit
            const float keepDistance = 1e-4f;   // avoid bad cases for GJK (penetration / exact hit)
            int         iterations   = 10;      // return after this many advances, regardless of accuracy
            float       fraction     = 0.0f;

            while (true)
            {
                if (fraction >= collector.MaxFraction)
                {
                    // Exceeded the maximum fraction without a hit
                    return(false);
                }

                // Find the current distance
                DistanceQueries.Result distanceResult = DistanceQueries.ConvexConvex(target, input.Collider, targetFromQuery);

                // Check for a hit
                if (distanceResult.Distance < tolerance || --iterations == 0)
                {
                    targetFromQuery.Translation = input.Start;
                    return(collector.AddHit(new ColliderCastHit
                    {
                        Position = distanceResult.PositionOnBinA,
                        SurfaceNormal = -distanceResult.NormalInA,
                        Fraction = fraction,
                        ColliderKey = ColliderKey.Empty,
                        RigidBodyIndex = -1
                    }));
                }

                // Check for a miss
                float dot = math.dot(distanceResult.NormalInA, input.Ray.Displacement);
                if (dot <= 0.0f)
                {
                    // Collider is moving away from the target, it will never hit
                    return(false);
                }

                // Advance
                fraction += (distanceResult.Distance - keepDistance) / dot;
                if (fraction >= collector.MaxFraction)
                {
                    // Exceeded the maximum fraction without a hit
                    return(false);
                }

                targetFromQuery.Translation = math.lerp(input.Start, input.End, fraction);
            }
        }
Exemplo n.º 13
0
 private static unsafe void CompositeConvex(
     Context context,
     Collider *compositeColliderA, Collider *convexColliderB, MTransform worldFromA, MTransform worldFromB,
     MotionExpansion expansion, bool flipped)
 {
     // Flip the relevant inputs and call convex-vs-composite
     expansion.Linear *= -1.0f;
     ConvexComposite(context, ColliderKey.Empty,
                     convexColliderB, compositeColliderA, worldFromB, worldFromA, expansion, !flipped);
 }
Exemplo n.º 14
0
        public static unsafe bool ColliderCollider <T>(ColliderCastInput input, Collider *target, ref T collector) where T : struct, ICollector <ColliderCastHit>
        {
            if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, target->Filter))
            {
                return(false);
            }

            if (!input.QueryContext.IsInitialized)
            {
                input.QueryContext = QueryContext.DefaultContext;
            }

            switch (input.Collider->CollisionType)
            {
            case CollisionType.Convex:
                switch (target->Type)
                {
                case ColliderType.Sphere:
                case ColliderType.Capsule:
                case ColliderType.Triangle:
                case ColliderType.Quad:
                case ColliderType.Box:
                case ColliderType.Cylinder:
                case ColliderType.Convex:
                    if (ConvexConvex(input, target, collector.MaxFraction, out ColliderCastHit hit))
                    {
                        return(collector.AddHit(hit));
                    }
                    return(false);

                case ColliderType.Mesh:
                    return(ConvexMesh(input, (MeshCollider *)target, ref collector));

                case ColliderType.Compound:
                    return(ConvexCompound(input, (CompoundCollider *)target, ref collector));

                case ColliderType.Terrain:
                    return(ConvexTerrain(input, (TerrainCollider *)target, ref collector));

                default:
                    SafetyChecks.ThrowNotImplementedException();
                    return(default);
                }

            case CollisionType.Composite:
            case CollisionType.Terrain:
                // no support for casting composite shapes
                SafetyChecks.ThrowNotImplementedException();
                return(default);

            default:
                SafetyChecks.ThrowNotImplementedException();
                return(default);
            }
        }
Exemplo n.º 15
0
        public static unsafe bool ColliderCollider <T>(ColliderDistanceInput input, Collider *target, ref T collector) where T : struct, ICollector <DistanceHit>
        {
            if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, target->Filter))
            {
                return(false);
            }

            switch (input.Collider->CollisionType)
            {
            case CollisionType.Convex:
                switch (target->Type)
                {
                case ColliderType.Convex:
                case ColliderType.Sphere:
                case ColliderType.Capsule:
                case ColliderType.Triangle:
                case ColliderType.Quad:
                case ColliderType.Box:
                case ColliderType.Cylinder:
                    MTransform targetFromQuery = new MTransform(input.Transform);
                    Result     result          = ConvexConvex(target, input.Collider, targetFromQuery);
                    if (result.Distance < collector.MaxFraction)
                    {
                        collector.AddHit(new DistanceHit
                        {
                            Fraction      = result.Distance,
                            SurfaceNormal = -result.NormalInA,
                            Position      = result.PositionOnAinA,
                            ColliderKey   = ColliderKey.Empty
                        });
                        return(true);
                    }
                    return(false);

                case ColliderType.Mesh:
                    return(ConvexMesh(input, (MeshCollider *)target, ref collector));

                case ColliderType.Compound:
                    return(ConvexCompound(input, (CompoundCollider *)target, ref collector));

                default:
                    throw new NotImplementedException();
                }

            case CollisionType.Composite:
                // no support for composite query shapes
                throw new NotImplementedException();

            default:
                throw new NotImplementedException();
            }
        }
Exemplo n.º 16
0
 /// <summary>
 /// Given the root Collider of a hierarchy and a ColliderKey referencing a child in that hierarchy,
 /// this function returns the ChildCollider requested.
 /// </summary>
 /// <param name="rootColliderPtr">A <see cref="Collider"/> at the root of a Collider hierarchy.</param>
 /// <param name="childColliderKey">A <see cref="ColliderKey"/> referencing a child Collider somewhere in the hierarchy below rootColliderPtr.</param>
 /// <param name="childCollider">A valid <see cref="ChildCollider"/> returned from the hierarchy, if found.</param>
 /// <returns>Whether a specified ColliderKey was successfully found in the hierarchy.</returns>
 public static unsafe bool TryGetChildInHierarchy(Collider *rootColliderPtr, ColliderKey childColliderKey, out ChildCollider childCollider)
 {
     //public static unsafe bool GetLeafCollider(Collider* root, RigidTransform rootTransform, ColliderKey key, out ChildCollider leaf)
     childCollider = new ChildCollider(rootColliderPtr, RigidTransform.identity);
     while (!childColliderKey.Equals(ColliderKey.Empty))
     {
         if (!childCollider.Collider->GetChild(ref childColliderKey, out ChildCollider child))
         {
             break;
         }
         childCollider = new ChildCollider(childCollider, child);
     }
     return(childCollider.Collider != null);
 }
Exemplo n.º 17
0
 public CompositeCompositeLeafCollector(
     Context context,
     Collider *compositeColliderA, Collider *compositeColliderB,
     MTransform worldFromA, MTransform worldFromB, MotionExpansion expansion, bool flipped)
 {
     m_Context            = context;
     m_CompositeColliderA = compositeColliderA;
     m_CompositeColliderB = compositeColliderB;
     m_WorldFromA         = worldFromA;
     m_WorldFromB         = worldFromB;
     m_Expansion          = expansion;
     m_Flipped            = flipped;
     m_KeyPath            = ColliderKeyPath.Empty;
 }
Exemplo n.º 18
0
        bool CollisionDetection(Collider *pCollider, ColliderEx *pColliderEx, Vector3 point1, Vector3 point2, out Vector3 pointOnLine, out Vector3 pointOnCollider, out float Radius)
        {
            if (pCollider->Height <= EPSILON)
            {
                var direction       = point2 - point1;
                var directionLength = direction.magnitude;
                direction /= directionLength;

                var toCenter         = pColliderEx->Position - point1;
                var dot              = Vector3.Dot(direction, toCenter);
                var pointOnDirection = direction * Mathf.Clamp(dot, 0.0f, directionLength);

                pointOnCollider = pColliderEx->Position;
                pointOnLine     = pointOnDirection + point1;
                Radius          = pCollider->RadiusHead;

                if ((pointOnCollider - pointOnLine).sqrMagnitude > pCollider->RadiusHead * pCollider->RadiusHead)
                {
                    return(false);
                }

                return(true);
            }
            else
            {
                var capsuleDir = pColliderEx->Direction;
                var capsulePos = pColliderEx->Position;
                var pointDir   = point2 - point1;

                float t1, t2;
                var   sqrDistance = ComputeNearestPoints(capsulePos, capsuleDir, point1, pointDir, out t1, out t2, out pointOnCollider, out pointOnLine);
                t1     = Mathf.Clamp01(t1);
                Radius = pCollider->RadiusHead + (pCollider->RadiusTail - pCollider->RadiusHead) * t1;

                if (sqrDistance > Radius * Radius)
                {
                    pointOnCollider = Vector3.zero;
                    pointOnLine     = Vector3.zero;
                    return(false);
                }

                t2 = Mathf.Clamp01(t2);

                pointOnCollider = capsulePos + capsuleDir * t1;
                pointOnLine     = point1 + pointDir * t2;

                return((pointOnCollider - pointOnLine).sqrMagnitude <= Radius * Radius);
            }
        }
Exemplo n.º 19
0
        internal static unsafe bool PointDistance <T>(PointDistanceInput input, Collider *collider, ref T collector) where T : struct, ICollector <DistanceHit>
        {
            if (!CollisionFilter.IsCollisionEnabled(input.Filter, collider->Filter))
            {
                return(false);
            }

            // Ensure the query context is initialized.
            input.QueryContext.EnsureIsInitialized();

            var           proxySource = new DistanceProxy(1, &input.Position, 0f);
            DistanceProxy proxyTarget;

            switch (collider->ColliderType)
            {
            case ColliderType.Box:
            case ColliderType.Polygon:
            case ColliderType.Capsule:
            case ColliderType.Circle:
            {
                var convexCollider = (ConvexCollider *)collider;
                proxyTarget = new DistanceProxy(ref convexCollider->m_ConvexHull);
                break;
            }

            case ColliderType.Compound:
                return(PointDistanceCompound(input, (PhysicsCompoundCollider *)collider, ref collector));

            default:
                SafetyChecks.ThrowNotImplementedException();
                return(default);
            }

            var hit = ColliderDistance(PhysicsTransform.Identity, ref proxySource, ref proxyTarget);

            if (hit.Distance < collector.MaxFraction)
            {
                hit.PhysicsBodyIndex = input.QueryContext.PhysicsBodyIndex;
                hit.ColliderKey      = input.QueryContext.ColliderKey;
                hit.Entity           = input.QueryContext.Entity;

                hit.PointA = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, hit.PointA);
                hit.PointB = PhysicsMath.mul(input.QueryContext.LocalToWorldTransform, hit.PointB);

                return(collector.AddHit(hit));
            }

            return(false);
        }
            public void Execute(ArchetypeChunk chunk, int chunkIndex, int firstEntityIndex)
            {
                NativeArray <KinematicMotor>  chunkMotors       = chunk.GetNativeArray(KinematicMotorType);
                NativeArray <Movement>        chunkMovements    = chunk.GetNativeArray(MovementType);
                NativeArray <PhysicsCollider> chunkColliders    = chunk.GetNativeArray(PhysicsColliderType);
                NativeArray <Translation>     chunkTranslations = chunk.GetNativeArray(TranslationType);
                NativeArray <Rotation>        chunkRotations    = chunk.GetNativeArray(RotationType);

                for (int i = 0; i < chunk.Count; i++)
                {
                    KinematicMotor  motor       = chunkMotors[i];
                    Movement        movement    = chunkMovements[i];
                    PhysicsCollider collider    = chunkColliders[i];
                    Translation     translation = chunkTranslations[i];
                    Rotation        rotation    = chunkRotations[i];

                    RigidTransform transform = new RigidTransform
                    {
                        pos = translation.Value,
                        rot = rotation.Value
                    };

                    float3 velocity = movement.Value;

                    unsafe
                    {
                        Collider *queryCollider;
                        {
                            Collider *colliderPtr = collider.ColliderPtr;

                            byte *copiedColliderMemory = stackalloc byte[colliderPtr->MemorySize];
                            queryCollider = (Collider *)(copiedColliderMemory);
                            UnsafeUtility.MemCpy(queryCollider, colliderPtr, colliderPtr->MemorySize);
                            queryCollider->Filter = CollisionFilter.Default;
                        }

                        KinematicMotorUtilities.SolveCollisionConstraints(World, DeltaTime, motor.MaxIterations, motor.SkinWidth, 360, queryCollider, ref transform, ref velocity, ref DistanceHits, ref ColliderCastHits, ref SurfaceConstraintInfos);
                    }

                    translation.Value = transform.pos;
                    movement.Value    = velocity;

                    // Apply data back to chunk
                    {
                        chunkTranslations[i] = translation;
                        chunkMovements[i]    = movement;
                    }
                }
            }
Exemplo n.º 21
0
 public ConvexCompositeOverlapCollector(
     Context context,
     Collider *convexCollider, ColliderKey convexColliderKey, Collider *compositeCollider,
     MTransform worldFromA, MTransform worldFromB, float collisionTolerance, bool flipped)
 {
     m_Context                  = context;
     m_ConvexColliderA          = convexCollider;
     m_ConvexColliderKey        = convexColliderKey;
     m_CompositeColliderB       = compositeCollider;
     m_CompositeColliderKeyPath = ColliderKeyPath.Empty;
     m_WorldFromA               = worldFromA;
     m_WorldFromB               = worldFromB;
     m_CollisionTolerance       = collisionTolerance;
     m_Flipped                  = flipped;
 }
Exemplo n.º 22
0
        /// <summary>
        /// Given the root Collider of a hierarchy and a ColliderKey referencing a child in that hierarchy,
        /// this function returns the ColliderKey referencing the parent Collider.
        /// </summary>
        /// <param name="rootColliderPtr">A <see cref="Collider"/> at the root of a Collider hierarchy.</param>
        /// <param name="childColliderKey">A <see cref="ColliderKey"/> referencing a child Collider somewhere in the hierarchy below rootColliderPtr.</param>
        /// <param name="parentColliderKey">A <see cref="ColliderKey"/> referencing the parent of the child Collider. Will be ColliderKey.Empty if the parameters where invalid.</param>
        /// <returns>Whether the parent was successfully found in the hierarchy.</returns>
        public static unsafe bool TryGetParentColliderKey(Collider *rootColliderPtr, ColliderKey childColliderKey, out ColliderKey parentColliderKey)
        {
            var childColliderPtr        = rootColliderPtr;
            var childColliderKeyNumBits = childColliderPtr->NumColliderKeyBits;

            // Start with an Empty collider key and push sub keys onto it as we traverse down the compound hierarchy.
            var parentColliderKeyPath = ColliderKeyPath.Empty;

            // On the way down, the childColliderKey pops of sub keys and pushes them onto the parentColliderKeyPath
            do
            {
                childColliderKey.PopSubKey(childColliderKeyNumBits, out var childIndex);
                switch (childColliderPtr->Type)
                {
                case ColliderType.Compound:
                    // Get the next child down and loop again
                    parentColliderKeyPath.PushChildKey(new ColliderKeyPath(new ColliderKey(childColliderKeyNumBits, childIndex), childColliderKeyNumBits));
                    childColliderPtr        = ((CompoundCollider *)childColliderPtr)->Children[(int)childIndex].Collider;
                    childColliderKeyNumBits = childColliderPtr->NumColliderKeyBits;
                    break;

                case ColliderType.Mesh:
                case ColliderType.Terrain:
                    // We've hit a Terrain or Mesh collider so there should only be PolygonColliders below this.
                    // At this point the childColliderKey should be Empty and childIndex should be the index of the polygon.
                    if (!childColliderKey.Equals(ColliderKey.Empty))
                    {
                        // We've reached the bottom without popping all the child keys.
                        // The given childColliderKey doesn't fit this hierarchy!
                        parentColliderKey = ColliderKey.Empty;
                        return(false);
                    }
                    break;

                default:
                    // We've hit a Convex collider, so rootColliderPtr must not have been
                    // the root of a hierarchy in the first place and so there is no parent!
                    parentColliderKey = ColliderKey.Empty;
                    return(false);
                }
            }while (!childColliderKey.Equals(ColliderKey.Empty) &&
                    !childColliderPtr->CollisionType.Equals(CollisionType.Convex));

            parentColliderKey = parentColliderKeyPath.Key;
            // childColliderKey should be Empty at this point.
            // However, if it isn't then we reached a leaf without finding the child collider!
            return(childColliderKey.Equals(ColliderKey.Empty));
        }
        public static unsafe bool IsTrigger(Collider *c, ColliderKey key)
        {
            bool bIsTrigger = false;

            {
                var cc = ((ConvexCollider *)c);
                if (cc->CollisionType != CollisionType.Convex)
                {
                    c->GetLeaf(key, out ChildCollider child);
                    cc = (ConvexCollider *)child.Collider;
                    UnityEngine.Assertions.Assert.IsTrue(cc->CollisionType == CollisionType.Convex);
                }
                bIsTrigger = cc->Material.IsTrigger;
            }
            return(bIsTrigger);
        }
Exemplo n.º 24
0
        public static unsafe bool ColliderCollider <T>(ColliderCastInput input, Collider *target, ref T collector) where T : struct, ICollector <ColliderCastHit>
        {
            if (!CollisionFilter.IsCollisionEnabled(input.Collider->Filter, target->Filter))
            {
                return(false);
            }

            switch (input.Collider->CollisionType)
            {
            case CollisionType.Convex:
                switch (target->Type)
                {
                case ColliderType.Sphere:
                case ColliderType.Capsule:
                case ColliderType.Triangle:
                case ColliderType.Quad:
                case ColliderType.Box:
                case ColliderType.Cylinder:
                case ColliderType.Convex:
                    return(ConvexConvex(input, target, ref collector));

                case ColliderType.Mesh:
                    return(ConvexMesh(input, (MeshCollider *)target, ref collector));

                case ColliderType.Compound:
                    return(ConvexCompound(input, (CompoundCollider *)target, ref collector));

                case ColliderType.Terrain:
                    return(ConvexTerrain(input, (TerrainCollider *)target, ref collector));

                default:
                    throw new NotImplementedException();
                }

            case CollisionType.Composite:
            case CollisionType.Terrain:
                // no support for casting composite shapes
                throw new NotImplementedException();

            default:
                throw new NotImplementedException();
            }
        }
        public unsafe void Execute <TCtx>(TCtx ctx, InputTriggerPort port) where TCtx : IGraphInstance
        {
            var entity = ctx.ReadEntity(Entity);

            if (entity == Unity.Entities.Entity.Null)
            {
                return;
            }

            if (!ctx.EntityManager.HasComponent <PhysicsCollider>(entity))
            {
                return;
            }

            PhysicsCollider physicsCollider = ctx.EntityManager.GetComponentData <PhysicsCollider>(entity);
            Collider *      colliderPtr     = (Collider *)physicsCollider.Value.GetUnsafePtr();

            colliderPtr->Filter = CollisionFilter.Default;

            ctx.Trigger(Output);
        }
Exemplo n.º 26
0
        public static unsafe void DrawCollider(
            Collider *collider,
            ref PhysicsTransform worldTransform,
            ref PhysicsDebugStreamSystem.Context outputStream,
            Color colliderColor)
        {
            switch (collider->CollisionType)
            {
            case CollisionType.Convex:
            {
                DrawConvex(collider, ref worldTransform, ref outputStream, colliderColor);
                return;
            }

            case CollisionType.Composite:
            {
                DrawComposite(collider, ref worldTransform, ref outputStream, colliderColor);
                return;
            }
            }
        }
Exemplo n.º 27
0
        static bool HitTest(Collider *pCollider, ColliderEx *pColliderEx, Vector3 point1, Vector3 point2, out Vector3 pointOnLine, out Vector3 pointOnCollider)
        {
            if (pCollider->IsSphere)
            {
                var direction       = point2 - point1;
                var directionLength = direction.magnitude;
                direction /= directionLength;

                var toCenter         = pColliderEx->Position - point1;
                var dot              = Vector3.Dot(direction, toCenter);
                var pointOnDirection = direction * Mathf.Clamp(dot, 0.0f, directionLength);

                pointOnCollider = pColliderEx->Position;
                pointOnLine     = pointOnDirection + point1;

                return(!((pointOnCollider - pointOnLine).sqrMagnitude > pCollider->Radius * pCollider->Radius));
            }
            else
            {
                var capsuleDir = pColliderEx->Direction;
                var capsulePos = pColliderEx->Position;
                var pointDir   = point2 - point1;

                var sqrDistance = ComputeNearestPoints(capsulePos, capsuleDir, point1, pointDir, out float t1, out float t2, out pointOnCollider, out pointOnLine);
                if (sqrDistance > pCollider->Radius * pCollider->Radius)
                {
                    pointOnCollider = Vector3.zero;
                    pointOnLine     = Vector3.zero;
                    return(false);
                }

                t1 = Mathf.Clamp01(t1);
                t2 = Mathf.Clamp01(t2);

                pointOnCollider = capsulePos + capsuleDir * t1;
                pointOnLine     = point1 + pointDir * t2;

                return((pointOnCollider - pointOnLine).sqrMagnitude <= pCollider->Radius * pCollider->Radius);
            }
        }
Exemplo n.º 28
0
        public unsafe void Execute <TCtx>(TCtx ctx, InputTriggerPort port) where TCtx : IGraphInstance
        {
            var entity = ctx.ReadEntity(Entity);

            if (entity == Unity.Entities.Entity.Null)
            {
                return;
            }

            if (!ctx.EntityManager.HasComponent <PhysicsMass>(entity) || !ctx.EntityManager.HasComponent <PhysicsCollider>(entity))
            {
                return;
            }

            PhysicsCollider physicsCollider = ctx.EntityManager.GetComponentData <PhysicsCollider>(entity);

            float newMass = ctx.ReadFloat(Mass);

            Collider *colliderPtr = (Collider *)physicsCollider.Value.GetUnsafePtr();

            ctx.EntityManager.SetComponentData(entity, PhysicsMass.CreateDynamic(colliderPtr->MassProperties, newMass));
        }
        public unsafe void Execute <TCtx>(TCtx ctx, InputTriggerPort port) where TCtx : IGraphInstance
        {
            var entity = ctx.ReadEntity(Entity);

            if (entity == Unity.Entities.Entity.Null)
            {
                return;
            }

            if (!ctx.EntityManager.HasComponent <PhysicsCollider>(entity))
            {
                return;
            }

            if (!ctx.EntityManager.HasComponent <PhysicsVelocity>(entity))
            {
                return;
            }

            PhysicsCollider physicsCollider = ctx.EntityManager.GetComponentData <PhysicsCollider>(entity);

            Collider *colliderPtr = (Collider *)physicsCollider.Value.GetUnsafePtr();

            float3 linearVelocity  = ctx.ReadFloat3(Linear);
            float3 angularVelocity = ctx.ReadFloat3(Angular);

            // TODO: MBRIAU: Make sure to understand exactly what's going on here
            // Calculate the angular velocity in local space from rotation and world angular velocity
            float3 angularVelocityLocal = math.mul(math.inverse(colliderPtr->MassProperties.MassDistribution.Transform.rot), angularVelocity);

            ctx.EntityManager.SetComponentData(entity, new PhysicsVelocity()
            {
                Linear  = linearVelocity,
                Angular = angularVelocityLocal
            });

            ctx.Trigger(Output);
        }
Exemplo n.º 30
0
        private static unsafe void DrawComposite(
            Collider *collider,
            ref PhysicsTransform worldTransform,
            ref PhysicsDebugStreamSystem.Context outputStream,
            Color colliderColor)
        {
            switch (collider->ColliderType)
            {
            case ColliderType.Compound:
            {
                var compoundCollider = (PhysicsCompoundCollider *)collider;
                var children         = compoundCollider->Children;

                for (var i = 0; i < children.Length; ++i)
                {
                    ref PhysicsCompoundCollider.Child child = ref children[i];

                    var colliderWorldTransform = PhysicsMath.mul(worldTransform, child.CompoundFromChild);
                    DrawCollider(children[i].Collider, ref colliderWorldTransform, ref outputStream, colliderColor);
                }

                return;
            }
            }