Esempio n. 1
0
        public static unsafe BlobAssetReference <Collider> Create(
            NativeArray <sfloat> heights, int2 size, float3 scale, CollisionMethod collisionMethod, CollisionFilter filter, Material material
            )
        {
            SafetyChecks.CheckInRangeAndThrow(size.x, new int2(2, int.MaxValue), nameof(size));
            SafetyChecks.CheckInRangeAndThrow(size.y, new int2(2, int.MaxValue), nameof(size));
            SafetyChecks.CheckFiniteAndPositiveAndThrow(scale, nameof(scale));

            // Allocate memory for the collider
            int totalSize = sizeof(TerrainCollider) + Terrain.CalculateDataSize(size);
            var collider  = (TerrainCollider *)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp);

            UnsafeUtility.MemClear(collider, totalSize);

            // Initialize the collider
            collider->m_Header.Type          = ColliderType.Terrain;
            collider->m_Header.CollisionType = (collisionMethod == CollisionMethod.Triangles) ? CollisionType.Composite : CollisionType.Terrain;
            collider->m_Header.Version       = 1;
            collider->m_Header.Magic         = 0xff;
            collider->m_Header.Filter        = filter;
            collider->Material   = material;
            collider->MemorySize = totalSize;
            collider->Terrain.Init(size, scale, (sfloat *)heights.GetUnsafePtr());

            var blob = BlobAssetReference <Collider> .Create(collider, totalSize);

            UnsafeUtility.Free(collider, Allocator.Temp);
            return(blob);
        }
Esempio n. 2
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;
            }
        }
Esempio n. 3
0
 public ref            T this[int index]
 {
     get
     {
         SafetyChecks.CheckIndexAndThrow(index, Length);
         return(ref UnsafeUtility.ArrayElementAsRef <T>((byte *)m_OffsetPtr + *m_OffsetPtr, index));
     }
 }
Esempio n. 4
0
        public static unsafe BlobAssetReference <Collider> CreateTriangle(float3 vertex0, float3 vertex1, float3 vertex2, CollisionFilter filter, Material material)
        {
            SafetyChecks.CheckFiniteAndThrow(vertex0, nameof(vertex0));
            SafetyChecks.CheckFiniteAndThrow(vertex1, nameof(vertex1));
            SafetyChecks.CheckFiniteAndThrow(vertex2, nameof(vertex2));

            var collider = new PolygonCollider();

            collider.InitAsTriangle(vertex0, vertex1, vertex2, filter, material);

            return(BlobAssetReference <Collider> .Create(&collider, UnsafeUtility.SizeOf <PolygonCollider>()));
        }
Esempio n. 5
0
        public static unsafe BlobAssetReference <Collider> CreateQuad(float3 vertex0, float3 vertex1, float3 vertex2, float3 vertex3, CollisionFilter filter, Material material)
        {
            SafetyChecks.CheckFiniteAndThrow(vertex0, nameof(vertex0));
            SafetyChecks.CheckFiniteAndThrow(vertex1, nameof(vertex1));
            SafetyChecks.CheckFiniteAndThrow(vertex2, nameof(vertex2));
            SafetyChecks.CheckFiniteAndThrow(vertex3, nameof(vertex3));
            SafetyChecks.CheckCoplanarAndThrow(vertex0, vertex1, vertex2, vertex3, nameof(vertex3));

            PolygonCollider collider = default;

            collider.InitAsQuad(vertex0, vertex1, vertex2, vertex3, filter, material);
            return(BlobAssetReference <Collider> .Create(&collider, UnsafeUtility.SizeOf <PolygonCollider>()));
        }
Esempio n. 6
0
        internal static unsafe JobHandle ScheduleUnityPhysicsBodyPairsJob <T>(T jobData, ISimulation simulation, ref PhysicsWorld world, JobHandle inputDeps)
            where T : struct, IBodyPairsJobBase
        {
            SafetyChecks.CheckAreEqualAndThrow(SimulationType.UnityPhysics, simulation.Type);

            var data = new BodyPairsJobData <T>
            {
                UserJobData         = jobData,
                PhasedDispatchPairs = ((Simulation)simulation).StepContext.PhasedDispatchPairs.AsDeferredJobArray(),
                Bodies = world.Bodies
            };
            var parameters = new JobsUtility.JobScheduleParameters(
#if UNITY_2020_2_OR_NEWER
                UnsafeUtility.AddressOf(ref data), BodyPairsJobProcess <T> .Initialize(), inputDeps, ScheduleMode.Single);
#else
                UnsafeUtility.AddressOf(ref data), BodyPairsJobProcess <T> .Initialize(), inputDeps, ScheduleMode.Batched);
#endif
            return(JobsUtility.Schedule(ref parameters));
        }
Esempio n. 7
0
        private unsafe void UpdateCachedBoundingRadius()
        {
            m_BoundingRadius = sfloat.Zero;
            float3 center = BoundingVolumeHierarchy.Domain.Center;

            for (int i = 0; i < NumChildren; i++)
            {
                ref Child child = ref Children[i];

                float3 childFromCenter = math.transform(math.inverse(child.CompoundFromChild), center);
                sfloat radius          = sfloat.Zero;

                switch (child.Collider->Type)
                {
                case ColliderType.Sphere:
                case ColliderType.Box:
                case ColliderType.Capsule:
                case ColliderType.Quad:
                case ColliderType.Triangle:
                case ColliderType.Cylinder:
                case ColliderType.Convex:
                    radius = ((ConvexCollider *)child.Collider)->CalculateBoundingRadius(childFromCenter);
                    break;

                case ColliderType.Compound:
                    radius = ((CompoundCollider *)child.Collider)->CalculateBoundingRadius(childFromCenter);
                    break;

                case ColliderType.Mesh:
                    radius = ((MeshCollider *)child.Collider)->CalculateBoundingRadius(childFromCenter);
                    break;

                case ColliderType.Terrain:
                    Aabb terrainAabb = ((TerrainCollider *)child.Collider)->CalculateAabb();
                    radius = math.length(math.max(math.abs(terrainAabb.Max - childFromCenter), math.abs(terrainAabb.Min - childFromCenter)));
                    break;

                default:
                    SafetyChecks.ThrowNotImplementedException();
                    break;
                }
                m_BoundingRadius = math.max(m_BoundingRadius, radius);
            }
Esempio n. 8
0
        // Schedules a trigger events job only for UnityPhysics simulation
        internal static unsafe JobHandle ScheduleUnityPhysicsTriggerEventsJob <T>(T jobData, ISimulation simulation, ref PhysicsWorld world, JobHandle inputDeps)
            where T : struct, ITriggerEventsJobBase
        {
            SafetyChecks.CheckAreEqualAndThrow(SimulationType.UnityPhysics, simulation.Type);

            var data = new TriggerEventJobData <T>
            {
                UserJobData = jobData,
                EventReader = ((Simulation)simulation).TriggerEvents
            };

            // Ensure the input dependencies include the end-of-simulation job, so events will have been generated
            inputDeps = JobHandle.CombineDependencies(inputDeps, simulation.FinalSimulationJobHandle);
#if UNITY_2020_2_OR_NEWER
            var parameters = new JobsUtility.JobScheduleParameters(UnsafeUtility.AddressOf(ref data), TriggerEventJobProcess <T> .Initialize(), inputDeps, ScheduleMode.Single);
#else
            var parameters = new JobsUtility.JobScheduleParameters(UnsafeUtility.AddressOf(ref data), TriggerEventJobProcess <T> .Initialize(), inputDeps, ScheduleMode.Batched);
#endif
            return(JobsUtility.Schedule(ref parameters));
        }
Esempio n. 9
0
        public static unsafe BlobAssetReference <Collider> Create(NativeArray <float3> vertices, NativeArray <int3> triangles, CollisionFilter filter, Material material)
        {
            SafetyChecks.CheckTriangleIndicesInRangeAndThrow(triangles, vertices.Length, nameof(triangles));

            // Copy vertices
            var tempVertices = new NativeArray <float3>(vertices, Allocator.Temp);

            // Triangle indices - needed for WeldVertices
            var tempIndices = new NativeArray <int>(triangles.Reinterpret <int>(UnsafeUtility.SizeOf <int3>()), Allocator.Temp);

            // Build connectivity and primitives

            NativeList <float3> uniqueVertices = MeshConnectivityBuilder.WeldVertices(tempIndices, tempVertices);

            var tempTriangleIndices = new NativeArray <int3>(triangles.Length, Allocator.Temp);

            UnsafeUtility.MemCpy(tempTriangleIndices.GetUnsafePtr(), tempIndices.GetUnsafePtr(), tempIndices.Length * UnsafeUtility.SizeOf <int>());

            var connectivity = new MeshConnectivityBuilder(tempTriangleIndices, uniqueVertices);
            NativeList <MeshConnectivityBuilder.Primitive> primitives = connectivity.EnumerateQuadDominantGeometry(tempTriangleIndices, uniqueVertices);

            // Build bounding volume hierarchy
            int nodeCount = math.max(primitives.Length * 2 + 1, 2); // We need at least two nodes - an "invalid" node and a root node.
            var nodes     = new NativeArray <BoundingVolumeHierarchy.Node>(nodeCount, Allocator.Temp);
            int numNodes  = 0;

            {
                // Prepare data for BVH
                var points = new NativeList <BoundingVolumeHierarchy.PointAndIndex>(primitives.Length, Allocator.Temp);
                var aabbs  = new NativeArray <Aabb>(primitives.Length, Allocator.Temp);

                for (int i = 0; i < primitives.Length; i++)
                {
                    MeshConnectivityBuilder.Primitive p = primitives[i];

                    // Skip degenerate triangles
                    if (MeshConnectivityBuilder.IsTriangleDegenerate(p.Vertices[0], p.Vertices[1], p.Vertices[2]))
                    {
                        continue;
                    }

                    aabbs[i] = Aabb.CreateFromPoints(p.Vertices);
                    points.Add(new BoundingVolumeHierarchy.PointAndIndex
                    {
                        Position = aabbs[i].Center,
                        Index    = i
                    });
                }

                var bvh = new BoundingVolumeHierarchy(nodes);

                bvh.Build(points.AsArray(), aabbs, out numNodes, useSah: true);
            }

            // Build mesh sections
            BoundingVolumeHierarchy.Node *nodesPtr = (BoundingVolumeHierarchy.Node *)nodes.GetUnsafePtr();
            MeshBuilder.TempSection       sections = MeshBuilder.BuildSections(nodesPtr, numNodes, primitives);

            // Allocate collider
            int meshDataSize      = Mesh.CalculateMeshDataSize(numNodes, sections.Ranges);
            int totalColliderSize = Math.NextMultipleOf(sizeof(MeshCollider), 16) + meshDataSize;

            MeshCollider *meshCollider = (MeshCollider *)UnsafeUtility.Malloc(totalColliderSize, 16, Allocator.Temp);

            // Initialize it
            {
                UnsafeUtility.MemClear(meshCollider, totalColliderSize);
                meshCollider->MemorySize = totalColliderSize;

                meshCollider->m_Header.Type          = ColliderType.Mesh;
                meshCollider->m_Header.CollisionType = CollisionType.Composite;
                meshCollider->m_Header.Version      += 1;
                meshCollider->m_Header.Magic         = 0xff;

                ref var mesh = ref meshCollider->Mesh;

                mesh.Init(nodesPtr, numNodes, sections, filter, material);
                mesh.UpdateCachedBoundingRadius();

                // Calculate combined filter
                meshCollider->m_Header.Filter = mesh.Sections.Length > 0 ? mesh.Sections[0].Filters[0] : CollisionFilter.Default;
                for (int i = 0; i < mesh.Sections.Length; ++i)
                {
                    for (var j = 0; j < mesh.Sections[i].Filters.Length; ++j)
                    {
                        var f = mesh.Sections[i].Filters[j];
                        meshCollider->m_Header.Filter = CollisionFilter.CreateUnion(meshCollider->m_Header.Filter, f);
                    }
                }

                meshCollider->m_Aabb             = meshCollider->Mesh.BoundingVolumeHierarchy.Domain;
                meshCollider->NumColliderKeyBits = meshCollider->Mesh.NumColliderKeyBits;
            }
Esempio n. 10
0
        // Create a compound collider containing an array of other colliders.
        // The source colliders are copied into the compound, so that it becomes one blob.
        public static unsafe BlobAssetReference <Collider> Create(NativeArray <ColliderBlobInstance> children)
        {
            SafetyChecks.CheckNotEmptyAndThrow(children, nameof(children));

            // Get the total required memory size for the compound plus all its children,
            // and the combined filter of all children
            // TODO: Verify that the size is enough
            int             totalSize = Math.NextMultipleOf16(UnsafeUtility.SizeOf <CompoundCollider>());
            CollisionFilter filter    = children[0].Collider.Value.Filter;
            var             srcToDestInstanceAddrs = new NativeHashMap <long, long>(children.Length, Allocator.Temp);

            for (var childIndex = 0; childIndex < children.Length; childIndex++)
            {
                var child       = children[childIndex];
                var instanceKey = (long)child.Collider.GetUnsafePtr();
                if (srcToDestInstanceAddrs.ContainsKey(instanceKey))
                {
                    continue;
                }
                totalSize += Math.NextMultipleOf16(child.Collider.Value.MemorySize);
                filter     = CollisionFilter.CreateUnion(filter, child.Collider.Value.Filter);
                srcToDestInstanceAddrs.Add(instanceKey, 0L);
            }
            totalSize += (children.Length + BoundingVolumeHierarchy.Constants.MaxNumTreeBranches) * UnsafeUtility.SizeOf <BoundingVolumeHierarchy.Node>();

            // Allocate the collider
            var compoundCollider = (CompoundCollider *)UnsafeUtility.Malloc(totalSize, 16, Allocator.Temp);

            UnsafeUtility.MemClear(compoundCollider, totalSize);
            compoundCollider->m_Header.Type          = ColliderType.Compound;
            compoundCollider->m_Header.CollisionType = CollisionType.Composite;
            compoundCollider->m_Header.Version       = 1;
            compoundCollider->m_Header.Magic         = 0xff;
            compoundCollider->m_Header.Filter        = filter;

            // Initialize children array
            Child *childrenPtr = (Child *)((byte *)compoundCollider + UnsafeUtility.SizeOf <CompoundCollider>());

            compoundCollider->m_ChildrenBlob.Offset = (int)((byte *)childrenPtr - (byte *)(&compoundCollider->m_ChildrenBlob.Offset));
            compoundCollider->m_ChildrenBlob.Length = children.Length;
            byte *end = (byte *)childrenPtr + UnsafeUtility.SizeOf <Child>() * children.Length;

            end = (byte *)Math.NextMultipleOf16((ulong)end);

            uint maxTotalNumColliderKeyBits = 0;

            // Copy children
            for (int i = 0; i < children.Length; i++)
            {
                Collider *collider       = (Collider *)children[i].Collider.GetUnsafePtr();
                var       srcInstanceKey = (long)collider;
                var       dstAddr        = srcToDestInstanceAddrs[srcInstanceKey];
                if (dstAddr == 0L)
                {
                    dstAddr = (long)end;
                    srcToDestInstanceAddrs[srcInstanceKey] = dstAddr;
                    UnsafeUtility.MemCpy(end, collider, collider->MemorySize);
                    end += Math.NextMultipleOf16(collider->MemorySize);
                }
                childrenPtr[i].m_ColliderOffset  = (int)((byte *)dstAddr - (byte *)(&childrenPtr[i].m_ColliderOffset));
                childrenPtr[i].CompoundFromChild = children[i].CompoundFromChild;

                maxTotalNumColliderKeyBits = math.max(maxTotalNumColliderKeyBits, collider->TotalNumColliderKeyBits);
            }

            // Build mass properties
            compoundCollider->MassProperties = compoundCollider->BuildMassProperties();

            // Build bounding volume
            int numNodes = compoundCollider->BuildBoundingVolume(out NativeArray <BoundingVolumeHierarchy.Node> nodes);
            int bvhSize  = numNodes * UnsafeUtility.SizeOf <BoundingVolumeHierarchy.Node>();

            compoundCollider->m_BvhNodesBlob.Offset = (int)(end - (byte *)(&compoundCollider->m_BvhNodesBlob.Offset));
            compoundCollider->m_BvhNodesBlob.Length = numNodes;
            UnsafeUtility.MemCpy(end, nodes.GetUnsafeReadOnlyPtr(), bvhSize);
            compoundCollider->UpdateCachedBoundingRadius();
            end += bvhSize;

            // Validate nesting level of composite colliders.
            compoundCollider->TotalNumColliderKeyBits = maxTotalNumColliderKeyBits + compoundCollider->NumColliderKeyBits;

            // If TotalNumColliderKeyBits is greater than 32, it means maximum nesting level of composite colliders has been breached.
            // ColliderKey has 32 bits so it can't handle infinite nesting of composite colliders.
            if (compoundCollider->TotalNumColliderKeyBits > 32)
            {
                SafetyChecks.ThrowArgumentException(nameof(children), "Composite collider exceeded maximum level of nesting!");
            }

            // Copy to blob asset
            int usedSize = (int)(end - (byte *)compoundCollider);

            UnityEngine.Assertions.Assert.IsTrue(usedSize < totalSize);
            compoundCollider->MemorySize = usedSize;
            var blob = BlobAssetReference <Collider> .Create(compoundCollider, usedSize);

            UnsafeUtility.Free(compoundCollider, Allocator.Temp);

            return(blob);
        }
Esempio n. 11
0
        // Schedule all the jobs for the simulation step.
        // Enqueued callbacks can choose to inject additional jobs at defined sync points.
        // multiThreaded defines which simulation type will be called:
        //     - true will result in default multithreaded simulation
        //     - false will result in a very small number of jobs (1 per physics step phase) that are scheduled sequentially
        // Behavior doesn't change regardless of the multiThreaded argument provided.
        public unsafe SimulationJobHandles ScheduleStepJobs(SimulationStepInput input, SimulationCallbacks callbacksIn, JobHandle inputDeps, bool multiThreaded = true)
        {
            SafetyChecks.CheckFiniteAndPositiveAndThrow(input.TimeStep, nameof(input.TimeStep));
            SafetyChecks.CheckInRangeAndThrow(input.NumSolverIterations, new int2(1, int.MaxValue), nameof(input.NumSolverIterations));

            // Dispose and reallocate input velocity buffer, if dynamic body count has increased.
            // Dispose previous collision and trigger event data streams.
            // New event streams are reallocated later when the work item count is known.
            JobHandle handle = SimulationContext.ScheduleReset(input, inputDeps, false);

            SimulationContext.TimeStep = input.TimeStep;

            StepContext = new StepContext();

            if (input.World.NumDynamicBodies == 0)
            {
                // No need to do anything, since nothing can move
                m_StepHandles = new SimulationJobHandles(handle);
                return(m_StepHandles);
            }

            SimulationCallbacks callbacks = callbacksIn ?? new SimulationCallbacks();

            // Find all body pairs that overlap in the broadphase
            var handles = input.World.CollisionWorld.ScheduleFindOverlapsJobs(
                out NativeStream dynamicVsDynamicBodyPairs, out NativeStream dynamicVsStaticBodyPairs, handle, multiThreaded);

            handle = handles.FinalExecutionHandle;
            var disposeHandle1     = handles.FinalDisposeHandle;
            var postOverlapsHandle = handle;

            // Sort all overlapping and jointed body pairs into phases
            handles = m_Scheduler.ScheduleCreatePhasedDispatchPairsJob(
                ref input.World, ref dynamicVsDynamicBodyPairs, ref dynamicVsStaticBodyPairs, handle,
                ref StepContext.PhasedDispatchPairs, out StepContext.SolverSchedulerInfo, multiThreaded);
            handle = handles.FinalExecutionHandle;
            var disposeHandle2 = handles.FinalDisposeHandle;

            // Apply gravity and copy input velocities at this point (in parallel with the scheduler, but before the callbacks)
            var applyGravityAndCopyInputVelocitiesHandle = Solver.ScheduleApplyGravityAndCopyInputVelocitiesJob(
                input.World.DynamicsWorld.MotionVelocities, SimulationContext.InputVelocities,
                input.TimeStep * input.Gravity, multiThreaded ? postOverlapsHandle : handle, multiThreaded);

            handle = JobHandle.CombineDependencies(handle, applyGravityAndCopyInputVelocitiesHandle);
            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateDispatchPairs, this, ref input.World, handle);

            // Create contact points & joint Jacobians
            handles = NarrowPhase.ScheduleCreateContactsJobs(ref input.World, input.TimeStep,
                                                             ref StepContext.Contacts, ref StepContext.Jacobians, ref StepContext.PhasedDispatchPairs, handle,
                                                             ref StepContext.SolverSchedulerInfo, multiThreaded);
            handle = handles.FinalExecutionHandle;
            var disposeHandle3 = handles.FinalDisposeHandle;

            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContacts, this, ref input.World, handle);

            // Create contact Jacobians
            handles = Solver.ScheduleBuildJacobiansJobs(ref input.World, input.TimeStep, input.Gravity, input.NumSolverIterations,
                                                        handle, ref StepContext.PhasedDispatchPairs, ref StepContext.SolverSchedulerInfo,
                                                        ref StepContext.Contacts, ref StepContext.Jacobians, multiThreaded);
            handle = handles.FinalExecutionHandle;
            var disposeHandle4 = handles.FinalDisposeHandle;

            handle = callbacks.Execute(SimulationCallbacks.Phase.PostCreateContactJacobians, this, ref input.World, handle);

            // Solve all Jacobians
            Solver.StabilizationData solverStabilizationData = new Solver.StabilizationData(input, SimulationContext);
            handles = Solver.ScheduleSolveJacobiansJobs(ref input.World.DynamicsWorld, input.TimeStep, input.NumSolverIterations,
                                                        ref StepContext.Jacobians, ref SimulationContext.CollisionEventDataStream, ref SimulationContext.TriggerEventDataStream,
                                                        ref StepContext.SolverSchedulerInfo, solverStabilizationData, handle, multiThreaded);
            handle = handles.FinalExecutionHandle;
            var disposeHandle5 = handles.FinalDisposeHandle;

            handle = callbacks.Execute(SimulationCallbacks.Phase.PostSolveJacobians, this, ref input.World, handle);

            // Integrate motions
            handle = Integrator.ScheduleIntegrateJobs(ref input.World.DynamicsWorld, input.TimeStep, handle, multiThreaded);

            // Synchronize the collision world
            if (input.SynchronizeCollisionWorld)
            {
                handle = input.World.CollisionWorld.ScheduleUpdateDynamicTree(ref input.World, input.TimeStep, input.Gravity, handle, multiThreaded);  // TODO: timeStep = 0?
            }

            // Return the final simulation handle
            m_StepHandles.FinalExecutionHandle = handle;

            // Different dispose logic for single threaded simulation compared to "standard" threading (multi threaded)
            if (!multiThreaded)
            {
                handle = dynamicVsDynamicBodyPairs.Dispose(handle);
                handle = dynamicVsStaticBodyPairs.Dispose(handle);
                handle = StepContext.PhasedDispatchPairs.Dispose(handle);
                handle = StepContext.Contacts.Dispose(handle);
                handle = StepContext.Jacobians.Dispose(handle);
                handle = StepContext.SolverSchedulerInfo.ScheduleDisposeJob(handle);

                m_StepHandles.FinalDisposeHandle = handle;
            }
            else
            {
                // Return the final handle, which includes disposing temporary arrays
                JobHandle *deps = stackalloc JobHandle[5]
                {
                    disposeHandle1,
                    disposeHandle2,
                    disposeHandle3,
                    disposeHandle4,
                    disposeHandle5
                };
                m_StepHandles.FinalDisposeHandle = JobHandleUnsafeUtility.CombineDependencies(deps, 5);
            }

            return(m_StepHandles);
        }
Esempio n. 12
0
        // Steps the simulation immediately on a single thread without spawning any jobs.
        public static void StepImmediate(SimulationStepInput input, ref SimulationContext simulationContext)
        {
            SafetyChecks.CheckFiniteAndPositiveAndThrow(input.TimeStep, nameof(input.TimeStep));
            SafetyChecks.CheckInRangeAndThrow(input.NumSolverIterations, new int2(1, int.MaxValue), nameof(input.NumSolverIterations));

            if (input.World.NumDynamicBodies == 0)
            {
                // No need to do anything, since nothing can move
                return;
            }

            // Inform the context of the timeStep
            simulationContext.TimeStep = input.TimeStep;

            // Find all body pairs that overlap in the broadphase
            var dynamicVsDynamicBodyPairs = new NativeStream(1, Allocator.Temp);
            var dynamicVsStaticBodyPairs  = new NativeStream(1, Allocator.Temp);
            {
                var dynamicVsDynamicBodyPairsWriter = dynamicVsDynamicBodyPairs.AsWriter();
                var dynamicVsStaticBodyPairsWriter  = dynamicVsStaticBodyPairs.AsWriter();
                input.World.CollisionWorld.FindOverlaps(ref dynamicVsDynamicBodyPairsWriter, ref dynamicVsStaticBodyPairsWriter);
            }

            // Create dispatch pairs
            var dispatchPairs = new NativeList <DispatchPairSequencer.DispatchPair>(Allocator.Temp);

            DispatchPairSequencer.CreateDispatchPairs(ref dynamicVsDynamicBodyPairs, ref dynamicVsStaticBodyPairs,
                                                      input.World.NumDynamicBodies, input.World.Joints, ref dispatchPairs);

            // Apply gravity and copy input velocities
            Solver.ApplyGravityAndCopyInputVelocities(input.World.DynamicsWorld.MotionVelocities,
                                                      simulationContext.InputVelocities, input.TimeStep * input.Gravity);

            // Narrow phase
            var contacts = new NativeStream(1, Allocator.Temp);
            {
                var contactsWriter = contacts.AsWriter();
                NarrowPhase.CreateContacts(ref input.World, dispatchPairs.AsArray(), input.TimeStep, ref contactsWriter);
            }

            // Build Jacobians
            var jacobians = new NativeStream(1, Allocator.Temp);

            {
                var contactsReader  = contacts.AsReader();
                var jacobiansWriter = jacobians.AsWriter();
                Solver.BuildJacobians(ref input.World, input.TimeStep, input.Gravity, input.NumSolverIterations,
                                      dispatchPairs.AsArray(), ref contactsReader, ref jacobiansWriter);
            }

            // Solve Jacobians
            {
                var jacobiansReader       = jacobians.AsReader();
                var collisionEventsWriter = simulationContext.CollisionEventDataStream.AsWriter();
                var triggerEventsWriter   = simulationContext.TriggerEventDataStream.AsWriter();
                Solver.StabilizationData solverStabilizationData = new Solver.StabilizationData(input, simulationContext);
                Solver.SolveJacobians(ref jacobiansReader, input.World.DynamicsWorld.MotionVelocities,
                                      input.TimeStep, input.NumSolverIterations, ref collisionEventsWriter, ref triggerEventsWriter, solverStabilizationData);
            }

            // Integrate motions
            Integrator.Integrate(input.World.DynamicsWorld.MotionDatas, input.World.DynamicsWorld.MotionVelocities, input.TimeStep);

            // Synchronize the collision world if asked for
            if (input.SynchronizeCollisionWorld)
            {
                input.World.CollisionWorld.UpdateDynamicTree(ref input.World, input.TimeStep, input.Gravity);
            }
        }
Esempio n. 13
0
 public void PopCompositeCollider(uint numCompositeKeyBits) => SafetyChecks.ThrowNotSupportedException();
Esempio n. 14
0
 public void PushCompositeCollider(ColliderKeyPath compositeKey) => SafetyChecks.ThrowNotSupportedException();
Esempio n. 15
0
 public unsafe void AddColliderKeys(ColliderKey *keys, int count) => SafetyChecks.ThrowNotSupportedException();