From 82e4e512c8f298a41b3a37216e4ddc8f03a8fb93 Mon Sep 17 00:00:00 2001 From: Tyler Date: Sat, 30 Mar 2024 18:25:30 -0500 Subject: [PATCH] Version 0.10.0 Alpha.1 [Prerelease] Warning: This commit hash may not be preserved! Debug state: Some collider pairs may explode on contact due to improper contact generation when using UnitySim. There is a known issue with the generation of the inertia tensor for compound colliders with extreme scale values. --- Calligraphics/Systems/GenerateGlyphsSystem.cs | 12 + Core/Authoring/ObjectAuthoringExtensions.cs | 2 +- Core/Authoring/SubsceneLoadOptions.cs | 20 + Core/Authoring/SubsceneLoadOptions.cs.meta | 11 + Core/Components/SceneComponents.cs | 5 + Core/Containers/Collections.meta | 8 + .../Collections/UnsafeParallelBlockList.cs | 756 +++++++ .../UnsafeParallelBlockList.cs.meta | 0 Core/Containers/CommandBuffers.meta | 8 + .../DestroyCommandBuffer.cs | 0 .../DestroyCommandBuffer.cs.meta | 0 .../DisableCommandBuffer.cs | 0 .../DisableCommandBuffer.cs.meta | 0 .../EnableCommandBuffer.cs | 0 .../EnableCommandBuffer.cs.meta | 0 .../EntityOperationCommandBuffer.cs | 4 +- .../EntityOperationCommandBuffer.cs.meta | 0 .../InstantiateCommandBuffer.cs | 0 .../InstantiateCommandBuffer.cs.meta | 0 Core/Containers/ThreadLocal.meta | 8 + .../ThreadLocal/ThreadStackAllocator.cs | 290 +++ .../ThreadLocal/ThreadStackAllocator.cs.meta | 11 + Core/Containers/UnsafeParallelBlockList.cs | 457 ---- .../InstantiateCommandBufferUntyped.cs | 46 +- Core/Math/Rng.cs | 79 + Core/Math/mathExtensions.cs | 11 +- Core/Systems/Scenes/SceneManagerSystem.cs | 42 +- Core/Utilities/CollectionsExtensions.cs | 7 + EntitiesExposed/ArchetypeChunkExposed.cs | 6 + .../BakingSystems/RendererBakingSystem.cs | 99 +- .../Authoring/SkinnedMeshSettingsAuthoring.cs | 17 + Kinemation/Components/InternalComponents.cs | 24 +- Kinemation/Components/MeshComponents.cs | 25 +- Kinemation/Components/SkeletonComponents.cs | 4 +- .../LatiosVertexSkinningNode.cs | 2 +- Kinemation/Resources/BatchSkinning.compute | 52 +- Kinemation/ShaderLibrary/VertexSkinning.hlsl | 26 +- .../Culling/GenerateBrgDrawCommandsSystem.cs | 163 +- Kinemation/Systems/KinemationSuperSystems.cs | 1 + .../Systems/LatiosEntitiesGraphicsSystem.cs | 8 +- .../Systems/PostBatching/PrepareLODsSystem.cs | 81 + .../PostBatching/PrepareLODsSystem.cs.meta | 11 + .../Utilities/RuntimeSkeletonBlobBuilders.cs | 272 +++ .../RuntimeSkeletonBlobBuilders.cs.meta | 11 + MyriAudio/Components/EffectsComponents.cs | 24 +- .../DSP/BuiltInEffects/VirtualOutputEffect.cs | 15 +- MyriAudio/DSP/DSPContexts.cs | 105 +- MyriAudio/DSP/DSPInterfaces.cs | 26 +- MyriAudio/DSP/Primitives/BrickwallLimiter.cs | 7 + MyriAudio/DSP/Primitives/SampleFrame.cs | 6 + MyriAudio/DSP/Primitives/SampleQueue.cs | 7 + MyriAudio/Internal/DspGraph/MyriMegaKernel.cs | 86 +- .../DspGraph/MyriMegaKernelMixdown.cs | 71 +- .../MyriMegaKernelProcessReceivedBuffers.cs | 253 ++- .../DspGraph/MyriMegaKernelSampleStacks.cs | 138 +- .../Internal/Interop/InteropStructures.cs | 133 +- .../SourceGen/SourceGenDispatchers.cs | 3 - .../CompoundColliderSmartBlobberSystem.cs | 113 +- .../ConvexColliderSmartBlobberSystem.cs | 11 +- .../TriMeshColliderSmartBlobberSystem.cs | 10 +- .../Physics/Components/ColliderPsyshock.cs | 75 +- .../Debug/PhysicsDebug.DrawCollider.cs | 16 +- .../Debug/PhysicsDebug.LogFindPairsStats.cs | 81 - .../UnitySim/UnitySimContactJacobians.cs | 569 +++++ .../UnitySim/UnitySimContactJacobians.cs.meta | 11 + .../Dynamics/UnitySim/UnitySimContacts.cs | 61 +- .../Dynamics/UnitySim/UnitySimMotion.cs | 166 ++ .../Dynamics/UnitySim/UnitySimMotion.cs.meta | 11 + .../UnitySim/UnitySimShapeMassUtilities.cs | 292 +++ .../UnitySimShapeMassUtilities.cs.meta | 11 + .../Builders/BuildCollisionLayerInternal.cs | 351 +++- .../Internal/Builders/ConvexHullBuilder.cs | 16 +- .../Physics/Internal/Math/Plane.cs | 3 +- .../Physics/Internal/Math/mathex.collision.cs | 102 + .../Physics/Internal/Math/mathex.plane.cs | 2 +- .../Queries/ColliderCollider/BoxBox.cs | 4 +- .../Queries/ColliderCollider/BoxCompound.cs | 8 +- .../Queries/ColliderCollider/BoxConvex.cs | 25 +- .../Queries/ColliderCollider/BoxTriMesh.cs | 17 +- .../Queries/ColliderCollider/BoxTriangle.cs | 10 +- .../ColliderCollider/CapsuleCompound.cs | 4 +- .../Queries/ColliderCollider/CapsuleConvex.cs | 6 +- .../ColliderCollider/CapsuleTriMesh.cs | 25 +- .../ColliderColliderDispatch.gen.cs | 355 ++-- .../ColliderColliderDispatch.tt | 41 +- .../ColliderCollider/CompoundCompound.cs | 15 +- .../ColliderCollider/ConvexCompound.cs | 12 +- .../Queries/ColliderCollider/ConvexConvex.cs | 5 +- .../Queries/ColliderCollider/ConvexTriMesh.cs | 17 +- .../Queries/ColliderCollider/SphereTriMesh.cs | 17 +- .../ColliderCollider/TriMeshCompound.cs | 12 +- .../ColliderCollider/TriMeshTriMesh.cs | 18 +- .../ColliderCollider/TriangleCompound.cs | 12 +- .../ColliderCollider/TriangleConvex.cs | 19 +- .../ColliderCollider/TriangleTriMesh.cs | 17 +- .../ColliderCollider/TriangleTriangle.cs | 6 +- .../Internal/Queries/Generalized/GjkEpa.cs | 10 +- .../Internal/Queries/Generalized/Mpr.cs | 6 +- .../Internal/Queries/InternalQueryTypes.cs | 10 + .../Internal/Queries/Layers/BurstEarlyInit.cs | 68 +- .../Queries/Layers/FindObjectsInternal.cs | 9 +- .../Queries/Layers/FindPairsInternal.cs | 1783 +++++++--------- .../Queries/Layers/FindPairsSweepMethods.cs | 1833 +++-------------- .../Queries/Layers/ForEachPairInternal.cs | 486 +++++ .../Layers/ForEachPairInternal.cs.meta | 11 + .../Queries/PointRayCollider/PointRayBox.cs | 12 +- .../PointRayCollider/PointRayDispatch.cs | 8 +- .../Builders/Physics.BuildCollisionLayer.cs | 496 +++-- .../Modifiers/Physics.ScaleCollider.cs | 16 +- .../Physics/Spatial/Queries/Physics.Aabbs.cs | 26 +- .../Queries/Physics.DistanceBetween.cs | 37 +- .../Spatial/Queries/Physics.FindObjects.cs | 8 +- .../Spatial/Queries/Physics.FindPairs.cs | 878 +++----- .../Spatial/Queries/Physics.ForEachPair.cs | 131 ++ .../Queries/Physics.ForEachPair.cs.meta | 11 + .../Colliders/CompoundColliderPsyshock.cs | 7 +- .../Types/Colliders/ConvexColliderPsyshock.cs | 5 + .../Physics/Types/CollisionLayer.cs | 118 +- PsyshockPhysics/Physics/Types/PairStream.cs | 1147 +++++++++++ .../Physics/Types/PairStream.cs.meta | 11 + .../Physics/Types/PhysicsComponentLookup.cs | 20 +- PsyshockPhysics/Physics/Types/QueryResults.cs | 26 + README.md | 13 +- .../Authoring/TransformBakeUtils.cs | 12 +- .../UnityTransforms/TransformSuperSystems.cs | 12 + .../UnityTransforms/TransformsBootstrap.cs | 20 + .../TransformsBootstrap.cs.meta | 11 + package.json | 2 +- 128 files changed, 8424 insertions(+), 4818 deletions(-) create mode 100644 Core/Authoring/SubsceneLoadOptions.cs create mode 100644 Core/Authoring/SubsceneLoadOptions.cs.meta create mode 100644 Core/Containers/Collections.meta create mode 100644 Core/Containers/Collections/UnsafeParallelBlockList.cs rename Core/Containers/{ => Collections}/UnsafeParallelBlockList.cs.meta (100%) create mode 100644 Core/Containers/CommandBuffers.meta rename Core/Containers/{ => CommandBuffers}/DestroyCommandBuffer.cs (100%) rename Core/Containers/{ => CommandBuffers}/DestroyCommandBuffer.cs.meta (100%) rename Core/Containers/{ => CommandBuffers}/DisableCommandBuffer.cs (100%) rename Core/Containers/{ => CommandBuffers}/DisableCommandBuffer.cs.meta (100%) rename Core/Containers/{ => CommandBuffers}/EnableCommandBuffer.cs (100%) rename Core/Containers/{ => CommandBuffers}/EnableCommandBuffer.cs.meta (100%) rename Core/Containers/{ => CommandBuffers}/EntityOperationCommandBuffer.cs (99%) rename Core/Containers/{ => CommandBuffers}/EntityOperationCommandBuffer.cs.meta (100%) rename Core/Containers/{ => CommandBuffers}/InstantiateCommandBuffer.cs (100%) rename Core/Containers/{ => CommandBuffers}/InstantiateCommandBuffer.cs.meta (100%) create mode 100644 Core/Containers/ThreadLocal.meta create mode 100644 Core/Containers/ThreadLocal/ThreadStackAllocator.cs create mode 100644 Core/Containers/ThreadLocal/ThreadStackAllocator.cs.meta delete mode 100644 Core/Containers/UnsafeParallelBlockList.cs create mode 100644 Kinemation/Systems/PostBatching/PrepareLODsSystem.cs create mode 100644 Kinemation/Systems/PostBatching/PrepareLODsSystem.cs.meta create mode 100644 Kinemation/Utilities/RuntimeSkeletonBlobBuilders.cs create mode 100644 Kinemation/Utilities/RuntimeSkeletonBlobBuilders.cs.meta create mode 100644 PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContactJacobians.cs create mode 100644 PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContactJacobians.cs.meta create mode 100644 PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimMotion.cs create mode 100644 PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimMotion.cs.meta create mode 100644 PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimShapeMassUtilities.cs create mode 100644 PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimShapeMassUtilities.cs.meta create mode 100644 PsyshockPhysics/Physics/Internal/Queries/Layers/ForEachPairInternal.cs create mode 100644 PsyshockPhysics/Physics/Internal/Queries/Layers/ForEachPairInternal.cs.meta create mode 100644 PsyshockPhysics/Physics/Spatial/Queries/Physics.ForEachPair.cs create mode 100644 PsyshockPhysics/Physics/Spatial/Queries/Physics.ForEachPair.cs.meta create mode 100644 PsyshockPhysics/Physics/Types/PairStream.cs create mode 100644 PsyshockPhysics/Physics/Types/PairStream.cs.meta create mode 100644 Transforms/UnityTransforms/TransformsBootstrap.cs create mode 100644 Transforms/UnityTransforms/TransformsBootstrap.cs.meta diff --git a/Calligraphics/Systems/GenerateGlyphsSystem.cs b/Calligraphics/Systems/GenerateGlyphsSystem.cs index 79badbc..192f0bf 100644 --- a/Calligraphics/Systems/GenerateGlyphsSystem.cs +++ b/Calligraphics/Systems/GenerateGlyphsSystem.cs @@ -21,6 +21,8 @@ public partial struct GenerateGlyphsSystem : ISystem EntityQuery m_singleFontQuery; EntityQuery m_multiFontQuery; + bool m_skipChangeFilter; + [BurstCompile] public void OnCreate(ref SystemState state) { @@ -31,6 +33,7 @@ public void OnCreate(ref SystemState state) .With(true) .With( false) .Build(); + m_skipChangeFilter = (state.WorldUnmanaged.Flags & WorldFlags.Editor) == WorldFlags.Editor; } [BurstCompile] @@ -45,6 +48,7 @@ public void OnUpdate(ref SystemState state) renderGlyphHandle = GetBufferTypeHandle(false), textBaseConfigurationHandle = GetComponentTypeHandle(true), textRenderControlHandle = GetComponentTypeHandle(false), + lastSystemVersion = m_skipChangeFilter ? 0 : state.LastSystemVersion }.ScheduleParallel(m_singleFontQuery, state.Dependency); } @@ -60,6 +64,8 @@ public partial struct Job : IJobChunk [ReadOnly] public ComponentTypeHandle textBaseConfigurationHandle; [ReadOnly] public ComponentTypeHandle fontBlobReferenceHandle; + public uint lastSystemVersion; + [NativeDisableContainerSafetyRestriction] private NativeList m_richTextTags; @@ -68,6 +74,12 @@ public partial struct Job : IJobChunk [BurstCompile] public void Execute(in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask) { + if (!(chunk.DidChange(ref glyphMappingMaskHandle, lastSystemVersion) || + chunk.DidChange(ref calliByteHandle, lastSystemVersion) || + chunk.DidChange(ref textBaseConfigurationHandle, lastSystemVersion) || + chunk.DidChange(ref fontBlobReferenceHandle, lastSystemVersion))) + return; + var calliBytesBuffers = chunk.GetBufferAccessor(ref calliByteHandle); var renderGlyphBuffers = chunk.GetBufferAccessor(ref renderGlyphHandle); var glyphMappingBuffers = chunk.GetBufferAccessor(ref glyphMappingElementHandle); diff --git a/Core/Authoring/ObjectAuthoringExtensions.cs b/Core/Authoring/ObjectAuthoringExtensions.cs index 701031d..2ece9f0 100644 --- a/Core/Authoring/ObjectAuthoringExtensions.cs +++ b/Core/Authoring/ObjectAuthoringExtensions.cs @@ -4,7 +4,7 @@ namespace Latios.Authoring { public static class ObjectAuthoringExtensions { - public static void DestroyDuringConversion(this Object unityEngineObject) + public static void DestroySafelyFromAnywhere(this Object unityEngineObject) { #if UNITY_EDITOR if (Application.isPlaying) diff --git a/Core/Authoring/SubsceneLoadOptions.cs b/Core/Authoring/SubsceneLoadOptions.cs new file mode 100644 index 0000000..9639e34 --- /dev/null +++ b/Core/Authoring/SubsceneLoadOptions.cs @@ -0,0 +1,20 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; + +namespace Latios +{ + [DisallowMultipleComponent] + [AddComponentMenu("Latios/Scene Management/Subscene Load Options")] + public class SubsceneLoadOptions : MonoBehaviour + { + public enum LoadOptions + { + Synchronous, + Asynchronous + } + + public LoadOptions loadOptions; + } +} + diff --git a/Core/Authoring/SubsceneLoadOptions.cs.meta b/Core/Authoring/SubsceneLoadOptions.cs.meta new file mode 100644 index 0000000..8b8ddac --- /dev/null +++ b/Core/Authoring/SubsceneLoadOptions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2f099d0f04fb5c24bbd0896c370587bb +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Core/Components/SceneComponents.cs b/Core/Components/SceneComponents.cs index 9975973..aa3a672 100644 --- a/Core/Components/SceneComponents.cs +++ b/Core/Components/SceneComponents.cs @@ -36,5 +36,10 @@ public struct CurrentScene : IComponentData public FixedString128Bytes previous => previousScene; public bool isFirstFrame => isSceneFirstFrame; } + + /// + /// Add this component to the subscene entity to disable forcing the subscene to load synchronously. + /// + public struct DisableSynchronousSubsceneLoadingTag : IComponentData { } } diff --git a/Core/Containers/Collections.meta b/Core/Containers/Collections.meta new file mode 100644 index 0000000..01a16b4 --- /dev/null +++ b/Core/Containers/Collections.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: d7f1cfabd3d55514db5ef2c591352092 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Core/Containers/Collections/UnsafeParallelBlockList.cs b/Core/Containers/Collections/UnsafeParallelBlockList.cs new file mode 100644 index 0000000..1544fc1 --- /dev/null +++ b/Core/Containers/Collections/UnsafeParallelBlockList.cs @@ -0,0 +1,756 @@ +using System.Diagnostics; +using System.Runtime.InteropServices; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Jobs.LowLevel.Unsafe; +using Unity.Mathematics; + +namespace Latios.Unsafe +{ + /// + /// An unsafe container which can be written to by multiple threads and multiple jobs. + /// The container is lock-free and items never change addresses once written. + /// Items written in the same thread in the same job will be read back in the same order. + /// This container is type-erased, but all elements are expected to be of the same size. + /// Unlike Unity's Unsafe* containers, it is safe to copy this type by value. + /// Alignment is guaranteed to be the greatest common factor of the element size and 16. + /// + public unsafe struct UnsafeParallelBlockList : INativeDisposable + { + private UnsafeIndexedBlockList m_blockList; + + /// + /// Construct a new UnsafeParallelBlockList using a UnityEngine allocator + /// + /// The size of each element in bytes that will be stored + /// + /// The number of elements stored per native thread index before needing to perform an additional allocation. + /// Higher values may allocate more memory that is left unused. Lower values may perform more allocations. + /// + /// The allocator to use for allocations + public UnsafeParallelBlockList(int elementSize, int elementsPerBlock, AllocatorManager.AllocatorHandle allocator) + { + m_blockList = new UnsafeIndexedBlockList(elementSize, elementsPerBlock, JobsUtility.MaxJobThreadCount, allocator); + } + + /// + /// The size of each element as defined when this instance was constructed. + /// + public int elementSize => m_blockList.elementSize; + + //The thread index is passed in because otherwise job reflection can't inject it through a pointer. + /// + /// Write an element for a given thread index + /// + /// It is assumed the size of T is the same as what was passed into elementSize during construction + /// The value to write + /// The thread index to use when writing. This should come from [NativeSetThreadIndex] or JobsUtility.ThreadIndex. + public void Write(T value, int threadIndex) where T : unmanaged + { + m_blockList.Write(value, threadIndex); + } + + /// + /// Reserve memory for an element and return the fixed memory address. + /// + /// The thread index to use when allocating. This should come from [NativeSetThreadIndex] or JobsUtility.ThreadIndex. + /// A pointer where an element can be copied to + public void* Allocate(int threadIndex) + { + return m_blockList.Allocate(threadIndex); + } + + /// + /// Count the number of elements. Do this once and cache the result. + /// + /// The number of elements stored + public int Count() + { + return m_blockList.Count(); + } + + /// + /// Returns true if the struct is not in a default uninitialized state. + /// This may report true incorrectly if the memory where this instance + /// exists was left uninitialized rather than cleared. + /// + public bool isCreated => m_blockList.isCreated; + + /// + /// Gets all the pointers for all elements stored. + /// This does not actually traverse the memory but instead calculates memory addresses from metadata, + /// which is often faster, especially for large elements. + /// + /// An array in which the pointers should be stored. Its Length should be equal to Count(). + public void GetElementPtrs(NativeArray ptrs) + { + m_blockList.GetElementPtrs(ptrs); + } + + /// + /// Copies all the elements stored into values. + /// + /// It is assumed the size of T is the same as what was passed into elementSize during construction + /// An array where the elements should be copied to. Its Length should be equal to Count(). + public void GetElementValues(NativeArray values) where T : struct + { + m_blockList.GetElementValues(values); + } + + /// + /// Copies all the elements from the blocklists into the contiguous memory region beginning at ptr. + /// + /// The first address of a contiguous memory region large enough to store all values in the blocklists + public void CopyElementsRaw(void* dstPtr) + { + m_blockList.CopyElementsRaw(dstPtr); + } + + /// + /// Uses a job to dispose this container + /// + /// A JobHandle for all jobs which should finish before disposal. + /// A JobHandle for the disposal job. + public JobHandle Dispose(JobHandle inputDeps) + { + return m_blockList.Dispose(inputDeps); + } + + /// + /// Disposes the container immediately. It is legal to call this from within a job, + /// as long as no other jobs or threads are using it. + /// + public void Dispose() + { + m_blockList.Dispose(); + } + + /// + /// Gets an enumerator for one of the thread indices in the job. + /// + /// + /// The thread index that was used when the elements were written. + /// This does not have to be the thread index of the reader. + /// In fact, you usually want to iterate through all threads. + /// + /// + public UnsafeIndexedBlockList.Enumerator GetEnumerator(int nativeThreadIndex) + { + return m_blockList.GetEnumerator(nativeThreadIndex); + } + + /// + /// Gets an enumerator for all thread indices + /// + public UnsafeIndexedBlockList.AllIndicesEnumerator GetEnumerator() + { + return m_blockList.GetEnumerator(); + } + } + + public unsafe struct UnsafeIndexedBlockList : INativeDisposable + { + [NativeDisableUnsafePtrRestriction] private PerIndexBlockList* m_perIndexBlockList; + + private readonly int m_elementSize; + private readonly int m_blockSize; + private readonly int m_elementsPerBlock; + private readonly int m_indexCount; + private AllocatorManager.AllocatorHandle m_allocator; + + /// + /// Construct a new UnsafeIndexedBlockList using a UnityEngine allocator + /// + /// The size of each element in bytes that will be stored + /// + /// The number of elements stored per native thread index before needing to perform an additional allocation. + /// Higher values may allocate more memory that is left unused. Lower values may perform more allocations. + /// + /// The number of stream indicecs in the block list. + /// The allocator to use for allocations + public UnsafeIndexedBlockList(int elementSize, int elementsPerBlock, int indexCount, AllocatorManager.AllocatorHandle allocator) + { + m_elementSize = elementSize; + m_elementsPerBlock = elementsPerBlock; + m_blockSize = elementSize * elementsPerBlock; + m_indexCount = indexCount; + m_allocator = allocator; + + m_perIndexBlockList = AllocatorManager.Allocate(allocator, m_indexCount); + for (int i = 0; i < m_indexCount; i++) + { + m_perIndexBlockList[i].blocks = default; + m_perIndexBlockList[i].lastByteAddressInBlock = null; + m_perIndexBlockList[i].nextWriteAddress = null; + m_perIndexBlockList[i].nextWriteAddress++; + m_perIndexBlockList[i].elementCount = 0; + } + } + + /// + /// The size of each element as defined when this instance was constructed. + /// + public int elementSize => m_elementSize; + + /// + /// The number of index streams in this instance. + /// + public int indexCount => m_indexCount; + + /// + /// Returns true if the struct is not in a default uninitialized state. + /// This may report true incorrectly if the memory where this instance + /// exists was left uninitialized rather than cleared. + /// + public bool isCreated => m_perIndexBlockList != null; + + /// + /// Write an element for a given index + /// + /// It is assumed the size of T is the same as what was passed into elementSize during construction + /// The value to write + /// The index to use when writing. + public void Write(T value, int index) where T : unmanaged + { + var blockList = m_perIndexBlockList + index; + if (blockList->nextWriteAddress > blockList->lastByteAddressInBlock) + { + if (!blockList->blocks.IsCreated) + { + blockList->blocks = new UnsafeList(8, m_allocator); + } + BlockPtr newBlockPtr = new BlockPtr + { + ptr = AllocatorManager.Allocate(m_allocator, m_blockSize) + }; + UnityEngine.Debug.Assert(CollectionHelper.IsAligned(newBlockPtr.ptr, 16)); + blockList->nextWriteAddress = newBlockPtr.ptr; + blockList->lastByteAddressInBlock = newBlockPtr.ptr + m_blockSize - 1; + blockList->blocks.Add(newBlockPtr); + } + + UnsafeUtility.CopyStructureToPtr(ref value, blockList->nextWriteAddress); + blockList->nextWriteAddress += m_elementSize; + blockList->elementCount++; + } + + /// + /// Reserve memory for an element and return the fixed memory address. + /// + /// The index to use when allocating. + /// A pointer where an element can be copied to + public void* Allocate(int index) + { + var blockList = m_perIndexBlockList + index; + if (blockList->nextWriteAddress > blockList->lastByteAddressInBlock) + { + if (!blockList->blocks.IsCreated) + { + blockList->blocks = new UnsafeList(8, m_allocator); + } + BlockPtr newBlockPtr = new BlockPtr + { + ptr = AllocatorManager.Allocate(m_allocator, m_blockSize), + }; + UnityEngine.Debug.Assert(CollectionHelper.IsAligned(newBlockPtr.ptr, 16)); + blockList->nextWriteAddress = newBlockPtr.ptr; + blockList->lastByteAddressInBlock = newBlockPtr.ptr + m_blockSize - 1; + blockList->blocks.Add(newBlockPtr); + } + + var result = blockList->nextWriteAddress; + blockList->nextWriteAddress += m_elementSize; + blockList->elementCount++; + return result; + } + + /// + /// Count the total number of elements across all indices. Do this once and cache the result. + /// + /// The number of elements stored + public int Count() + { + int result = 0; + for (int i = 0; i < m_indexCount; i++) + { + result += m_perIndexBlockList[i].elementCount; + } + return result; + } + + /// + /// Count the number of elements for a specific index. + /// + /// The index at which to count the number of elements within + /// The number of elements added to the specified index + public int CountForIndex(int index) + { + return m_perIndexBlockList[index].elementCount; + } + + /// + /// A pointer to an element stored + /// + public struct ElementPtr + { + public byte* ptr; + } + + /// + /// Gets all the pointers for all elements stored. + /// This does not actually traverse the memory but instead calculates memory addresses from metadata, + /// which is often faster, especially for large elements. + /// + /// An array in which the pointers should be stored. Its Length should be equal to Count(). + public void GetElementPtrs(NativeArray ptrs) + { + int dst = 0; + + for (int threadBlockId = 0; threadBlockId < m_indexCount; threadBlockId++) + { + var blockList = m_perIndexBlockList + threadBlockId; + if (blockList->elementCount > 0) + { + int src = 0; + for (int blockId = 0; blockId < blockList->blocks.Length - 1; blockId++) + { + var address = blockList->blocks[blockId].ptr; + for (int i = 0; i < m_elementsPerBlock; i++) + { + ptrs[dst] = new ElementPtr { ptr = address }; + address += m_elementSize; + src++; + dst++; + } + } + { + var address = blockList->blocks[blockList->blocks.Length - 1].ptr; + for (int i = src; i < blockList->elementCount; i++) + { + ptrs[dst] = new ElementPtr { ptr = address }; + address += m_elementSize; + dst++; + } + } + } + } + } + + /// + /// Copies all the elements stored into values. + /// + /// It is assumed the size of T is the same as what was passed into elementSize during construction + /// An array where the elements should be copied to. Its Length should be equal to Count(). + public void GetElementValues(NativeArray values) where T : struct + { + int dst = 0; + + for (int threadBlockId = 0; threadBlockId < m_indexCount; threadBlockId++) + { + var blockList = m_perIndexBlockList + threadBlockId; + if (blockList->elementCount > 0) + { + int src = 0; + CheckBlockCountMatchesCount(blockList->elementCount, blockList->blocks.Length); + for (int blockId = 0; blockId < blockList->blocks.Length - 1; blockId++) + { + var address = blockList->blocks[blockId].ptr; + for (int i = 0; i < m_elementsPerBlock; i++) + { + UnsafeUtility.CopyPtrToStructure(address, out T temp); + values[dst] = temp; + address += m_elementSize; + src++; + dst++; + } + } + { + var address = blockList->blocks[blockList->blocks.Length - 1].ptr; + for (int i = src; i < blockList->elementCount; i++) + { + UnsafeUtility.CopyPtrToStructure(address, out T temp); + values[dst] = temp; + address += m_elementSize; + dst++; + } + } + } + } + } + + /// + /// Copies all the elements from the blocklists into the contiguous memory region beginning at ptr. + /// + /// The first address of a contiguous memory region large enough to store all values in the blocklists + public void CopyElementsRaw(void* dstPtr) + { + byte* dst = (byte*)dstPtr; + + for (int threadBlockId = 0; threadBlockId < m_indexCount; threadBlockId++) + { + var blockList = m_perIndexBlockList + threadBlockId; + if (blockList->elementCount > 0) + { + int src = 0; + CheckBlockCountMatchesCount(blockList->elementCount, blockList->blocks.Length); + for (int blockId = 0; blockId < blockList->blocks.Length - 1; blockId++) + { + var address = blockList->blocks[blockId].ptr; + UnsafeUtility.MemCpy(dst, address, m_blockSize); + dst += m_blockSize; + src += m_elementsPerBlock; + } + { + var address = blockList->blocks[blockList->blocks.Length - 1].ptr; + if (src < blockList->elementCount) + UnsafeUtility.MemCpy(dst, address, (blockList->elementCount - src) * m_elementSize); + } + } + } + } + + /// + /// Steals elements from the other UnsafeIndexedBlockList with the same block sizes and allocator and + /// adds them to this instance at the same indices. Relative ordering of elements and memory addresses + /// in the other UnsafeIndexedBlockList may not be preserved. + /// + /// The other UnsafeIndexedBlockList to steal from + public void ConcatenateAndStealFromUnordered(ref UnsafeIndexedBlockList other) + { + CheckBlockListsMatch(ref other); + for (int threadBlockId = 0; threadBlockId < m_indexCount; threadBlockId++) + { + var blockList = m_perIndexBlockList + threadBlockId; + var otherBlockList = other.m_perIndexBlockList + threadBlockId; + + ConcatenateBlockList(blockList, otherBlockList, m_allocator); + } + } + + /// + /// Moves elements from one index into another. Relative ordering of elements and memory addresses + /// from the source index may not be preserved. + /// + /// The index where elements should be moved away from + /// The index where elements should be moved to + public void MoveIndexToOtherIndexUnordered(int sourceIndex, int destinationIndex) + { + ConcatenateBlockList(m_perIndexBlockList + destinationIndex, m_perIndexBlockList + sourceIndex, m_allocator); + } + + /// + /// Removes all elements from an index + /// + /// + public void ClearIndex(int index) + { + var blockList = m_perIndexBlockList + index; + if (blockList->blocks.IsCreated) + { + foreach (var block in blockList->blocks) + { + AllocatorManager.Free(m_allocator, block.ptr, m_blockSize); + } + + blockList->blocks.Clear(); + blockList->elementCount = 0; + blockList->lastByteAddressInBlock = null; + blockList->nextWriteAddress = null; + blockList->nextWriteAddress++; + } + } + + void ConcatenateBlockList(PerIndexBlockList* blockList, PerIndexBlockList* otherBlockList, AllocatorManager.AllocatorHandle otherAllocator) + { + if (otherBlockList->elementCount == 0) + return; + + if (blockList->elementCount == 0) + { + (*blockList, *otherBlockList) = (*otherBlockList, *blockList); + return; + } + + var elementsInLastBlock = blockList->elementCount % m_elementsPerBlock; + var elementsStillNeededInBlock = math.select(m_elementsPerBlock - elementsInLastBlock, 0, elementsInLastBlock == 0); + var elementsInOtherLastBlock = otherBlockList->elementCount % m_elementsPerBlock; + if (elementsInOtherLastBlock <= elementsStillNeededInBlock) + { + var otherBlock = otherBlockList->blocks[otherBlockList->blocks.Length - 1]; + var src = otherBlock.ptr; + var blockToAppend = blockList->blocks[blockList->blocks.Length - 1]; + var dst = blockToAppend.ptr + elementsInLastBlock * m_elementSize; + UnsafeUtility.MemCpy(dst, src, elementsInOtherLastBlock * m_elementSize); + AllocatorManager.Free(otherAllocator, otherBlock.ptr, m_blockSize); + elementsStillNeededInBlock -= elementsInOtherLastBlock; + otherBlockList->blocks.Length--; + blockList->nextWriteAddress += elementsInOtherLastBlock * m_elementSize; + elementsInOtherLastBlock = math.select(m_elementsPerBlock, 0, otherBlockList->blocks.Length == 0); + } + if (elementsInOtherLastBlock > elementsStillNeededInBlock) + { + var indexToStealFrom = elementsInOtherLastBlock - elementsStillNeededInBlock; + var otherBlock = otherBlockList->blocks[otherBlockList->blocks.Length - 1]; + var src = otherBlock.ptr + indexToStealFrom * m_elementSize; + var blockToAppend = blockList->blocks[blockList->blocks.Length - 1]; + var dst = blockToAppend.ptr + elementsInLastBlock * m_elementSize; + if (elementsStillNeededInBlock > 0) + UnsafeUtility.MemCpy(dst, src, elementsStillNeededInBlock * m_elementSize); + otherBlockList->nextWriteAddress = src; + otherBlockList->lastByteAddressInBlock = otherBlock.ptr + m_blockSize - 1; + } + if (otherBlockList->blocks.Length > 0) + { + blockList->blocks.AddRange(otherBlockList->blocks); + blockList->nextWriteAddress = otherBlockList->nextWriteAddress; + blockList->lastByteAddressInBlock = otherBlockList->lastByteAddressInBlock; + } + blockList->elementCount += otherBlockList->elementCount; + + otherBlockList->blocks.Clear(); + otherBlockList->elementCount = 0; + otherBlockList->lastByteAddressInBlock = null; + otherBlockList->nextWriteAddress = null; + otherBlockList->nextWriteAddress++; + } + + //This catches race conditions if I accidentally pass in 0 for thread index in the parallel writer because copy and paste. + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckBlockCountMatchesCount(int count, int blockCount) + { + int expectedBlocks = count / m_elementsPerBlock; + if (count % m_elementsPerBlock > 0) + expectedBlocks++; + if (blockCount != expectedBlocks) + throw new System.InvalidOperationException($"Block count: {blockCount} does not match element count: {count}"); + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckBlockListsMatch(ref UnsafeIndexedBlockList other) + { + if (m_blockSize != other.m_blockSize || m_elementSize != other.m_elementSize || m_indexCount != other.m_indexCount || m_allocator != other.m_allocator) + throw new System.InvalidOperationException("UnsafeIndexedBlockLists do not match."); + } + + [BurstCompile] + struct DisposeJob : IJob + { + public UnsafeIndexedBlockList uibl; + + public void Execute() + { + uibl.Dispose(); + } + } + + /// + /// Uses a job to dispose this container + /// + /// A JobHandle for all jobs which should finish before disposal. + /// A JobHandle for the disposal job. + public JobHandle Dispose(JobHandle inputDeps) + { + var jh = new DisposeJob { uibl = this }.Schedule(inputDeps); + m_perIndexBlockList = null; + return jh; + } + + /// + /// Disposes the container immediately. It is legal to call this from within a job, + /// as long as no other jobs or threads are using it. + /// + public void Dispose() + { + for (int i = 0; i < m_indexCount; i++) + { + if (m_perIndexBlockList[i].elementCount > 0) + { + for (int j = 0; j < m_perIndexBlockList[i].blocks.Length; j++) + { + var block = m_perIndexBlockList[i].blocks[j]; + AllocatorManager.Free(m_allocator, block.ptr, m_blockSize); + } + m_perIndexBlockList[i].blocks.Dispose(); + } + } + AllocatorManager.Free(m_allocator, m_perIndexBlockList, m_indexCount); + } + + private struct BlockPtr + { + public byte* ptr; + } + + [StructLayout(LayoutKind.Sequential, Size = 64)] + private struct PerIndexBlockList + { + public UnsafeList blocks; + public byte* nextWriteAddress; + public byte* lastByteAddressInBlock; + public int elementCount; + } + + /// + /// Gets an enumerator for one of the indices in the job. + /// + /// + /// The index that was used when the elements were written. + /// + public Enumerator GetEnumerator(int index) + { + return new Enumerator(m_perIndexBlockList + index, m_elementSize, m_elementsPerBlock); + } + + /// + /// An enumerator which can be used for iterating over the elements written by a single index. + /// It is allowed to have multiple enumerators for the same thread index. + /// + public struct Enumerator + { + private PerIndexBlockList* m_perThreadBlockList; + private byte* m_readAddress; + private byte* m_lastByteAddressInBlock; + private int m_blockIndex; + private int m_elementSize; + private int m_elementsPerBlock; + + internal Enumerator(void* perThreadBlockList, int elementSize, int elementsPerBlock) + { + m_perThreadBlockList = (PerIndexBlockList*)perThreadBlockList; + m_readAddress = null; + m_readAddress++; + m_lastByteAddressInBlock = null; + m_blockIndex = -1; + m_elementSize = elementSize; + m_elementsPerBlock = elementsPerBlock; + } + + /// + /// Advance to the next element + /// + /// Returns false if the previous element was the last, true otherwise + public bool MoveNext() + { + m_readAddress += m_elementSize; + if (m_readAddress > m_lastByteAddressInBlock) + { + m_blockIndex++; + if (m_perThreadBlockList->elementCount == 0 || m_blockIndex >= m_perThreadBlockList->blocks.Length) + return false; + + int elementsInBlock = math.min(m_elementsPerBlock, m_perThreadBlockList->elementCount - m_blockIndex * m_elementsPerBlock); + var blocks = m_perThreadBlockList->blocks.Ptr; + m_lastByteAddressInBlock = blocks[m_blockIndex].ptr + elementsInBlock * m_elementSize - 1; + m_readAddress = blocks[m_blockIndex].ptr; + } + return true; + } + + /// + /// Retrieves the current element, copying it to a variable of the specified type. + /// + /// It is assumed the size of T is the same as what was passed into elementSize during construction + /// A value containing a copy of the element stored, reinterpreted with the strong type + public T GetCurrent() where T : unmanaged + { + UnsafeUtility.CopyPtrToStructure(m_readAddress, out T t); + return t; + } + + /// + /// Retrieves the current element by ref of the specified type. + /// + /// It is assumed the size of T is the same as what was passed into elementSize during construction + /// A ref of the element stored, reinterpreted with the strong type + public ref T GetCurrentAsRef() where T : unmanaged + { + return ref UnsafeUtility.AsRef(m_readAddress); + } + + internal Enumerator GetNextIndexEnumerator() + { + return new Enumerator(m_perThreadBlockList + 1, m_elementSize, m_elementsPerBlock); + } + } + + /// + /// Gets an enumerator for all indices. + /// + public AllIndicesEnumerator GetEnumerator() + { + return new AllIndicesEnumerator(new Enumerator(m_perIndexBlockList, m_elementSize, m_elementsPerBlock), m_indexCount); + } + + /// + /// An enumerator which can be used for iterating over the elements written by all indices. + /// + public struct AllIndicesEnumerator + { + Enumerator m_enumerator; + int m_index; + int m_indexCount; + + internal AllIndicesEnumerator(Enumerator thread0Enumerator, int indexCount) + { + m_enumerator = thread0Enumerator; + m_index = 0; + m_indexCount = indexCount; + } + + /// + /// Advance to the next element + /// + /// Returns false if the previous element was the last, true otherwise + public bool MoveNext() + { + while (!m_enumerator.MoveNext()) + { + m_index++; + if (m_index >= m_indexCount) + return false; + m_enumerator = m_enumerator.GetNextIndexEnumerator(); + } + return true; + } + + /// + /// Retrieves the current element, copying it to a variable of the specified type. + /// + /// It is assumed the size of T is the same as what was passed into elementSize during construction + /// A value containing a copy of the element stored, reinterpreted with the strong type + public T GetCurrent() where T : unmanaged => m_enumerator.GetCurrent(); + + /// + /// Retrieves the current element by ref of the specified type. + /// + /// It is assumed the size of T is the same as what was passed into elementSize during construction + /// A ref of the element stored, reinterpreted with the strong type + public ref T GetCurrentAsRef() where T : unmanaged => ref m_enumerator.GetCurrentAsRef(); + } + } + + // Schedule for 128 iterations + //[BurstCompile] + //struct ExampleReadJob : IJobFor + //{ + // [NativeDisableUnsafePtrRestriction] public UnsafeParallelBlockList blockList; + // + // public void Execute(int index) + // { + // var enumerator = blockList.GetEnumerator(index); + // + // while (enumerator.MoveNext()) + // { + // int number = enumerator.GetCurrent(); + // + // if (number == 381) + // UnityEngine.Debug.Log("You found me!"); + // else if (number == 380) + // UnityEngine.Debug.Log("Where?"); + // } + // } + //} +} + diff --git a/Core/Containers/UnsafeParallelBlockList.cs.meta b/Core/Containers/Collections/UnsafeParallelBlockList.cs.meta similarity index 100% rename from Core/Containers/UnsafeParallelBlockList.cs.meta rename to Core/Containers/Collections/UnsafeParallelBlockList.cs.meta diff --git a/Core/Containers/CommandBuffers.meta b/Core/Containers/CommandBuffers.meta new file mode 100644 index 0000000..8cd3b35 --- /dev/null +++ b/Core/Containers/CommandBuffers.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 7b47039f45736114fa878d121ee69151 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Core/Containers/DestroyCommandBuffer.cs b/Core/Containers/CommandBuffers/DestroyCommandBuffer.cs similarity index 100% rename from Core/Containers/DestroyCommandBuffer.cs rename to Core/Containers/CommandBuffers/DestroyCommandBuffer.cs diff --git a/Core/Containers/DestroyCommandBuffer.cs.meta b/Core/Containers/CommandBuffers/DestroyCommandBuffer.cs.meta similarity index 100% rename from Core/Containers/DestroyCommandBuffer.cs.meta rename to Core/Containers/CommandBuffers/DestroyCommandBuffer.cs.meta diff --git a/Core/Containers/DisableCommandBuffer.cs b/Core/Containers/CommandBuffers/DisableCommandBuffer.cs similarity index 100% rename from Core/Containers/DisableCommandBuffer.cs rename to Core/Containers/CommandBuffers/DisableCommandBuffer.cs diff --git a/Core/Containers/DisableCommandBuffer.cs.meta b/Core/Containers/CommandBuffers/DisableCommandBuffer.cs.meta similarity index 100% rename from Core/Containers/DisableCommandBuffer.cs.meta rename to Core/Containers/CommandBuffers/DisableCommandBuffer.cs.meta diff --git a/Core/Containers/EnableCommandBuffer.cs b/Core/Containers/CommandBuffers/EnableCommandBuffer.cs similarity index 100% rename from Core/Containers/EnableCommandBuffer.cs rename to Core/Containers/CommandBuffers/EnableCommandBuffer.cs diff --git a/Core/Containers/EnableCommandBuffer.cs.meta b/Core/Containers/CommandBuffers/EnableCommandBuffer.cs.meta similarity index 100% rename from Core/Containers/EnableCommandBuffer.cs.meta rename to Core/Containers/CommandBuffers/EnableCommandBuffer.cs.meta diff --git a/Core/Containers/EntityOperationCommandBuffer.cs b/Core/Containers/CommandBuffers/EntityOperationCommandBuffer.cs similarity index 99% rename from Core/Containers/EntityOperationCommandBuffer.cs rename to Core/Containers/CommandBuffers/EntityOperationCommandBuffer.cs index b1d1fe6..caf9117 100644 --- a/Core/Containers/EntityOperationCommandBuffer.cs +++ b/Core/Containers/CommandBuffers/EntityOperationCommandBuffer.cs @@ -98,8 +98,8 @@ public void Execute() /// /// Disposes the EntityOperationCommandBuffer after the jobs which use it have finished. /// - /// The JobHandle for any jobs previously using this EnableCommandBuffer - /// + /// The JobHandle for any jobs previously using this EntityOperationCommandBuffer + /// The JobHandle for the disposing job scheduled public JobHandle Dispose(JobHandle inputDeps) { var jobHandle = new DisposeJob diff --git a/Core/Containers/EntityOperationCommandBuffer.cs.meta b/Core/Containers/CommandBuffers/EntityOperationCommandBuffer.cs.meta similarity index 100% rename from Core/Containers/EntityOperationCommandBuffer.cs.meta rename to Core/Containers/CommandBuffers/EntityOperationCommandBuffer.cs.meta diff --git a/Core/Containers/InstantiateCommandBuffer.cs b/Core/Containers/CommandBuffers/InstantiateCommandBuffer.cs similarity index 100% rename from Core/Containers/InstantiateCommandBuffer.cs rename to Core/Containers/CommandBuffers/InstantiateCommandBuffer.cs diff --git a/Core/Containers/InstantiateCommandBuffer.cs.meta b/Core/Containers/CommandBuffers/InstantiateCommandBuffer.cs.meta similarity index 100% rename from Core/Containers/InstantiateCommandBuffer.cs.meta rename to Core/Containers/CommandBuffers/InstantiateCommandBuffer.cs.meta diff --git a/Core/Containers/ThreadLocal.meta b/Core/Containers/ThreadLocal.meta new file mode 100644 index 0000000..bfaabe8 --- /dev/null +++ b/Core/Containers/ThreadLocal.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: bc9cd93398af13e4f9e7e7fb1a3c0acc +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Core/Containers/ThreadLocal/ThreadStackAllocator.cs b/Core/Containers/ThreadLocal/ThreadStackAllocator.cs new file mode 100644 index 0000000..55085de --- /dev/null +++ b/Core/Containers/ThreadLocal/ThreadStackAllocator.cs @@ -0,0 +1,290 @@ +using System; +using System.Diagnostics; +using System.Runtime.InteropServices; +using Unity.Burst; +using Unity.Burst.CompilerServices; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs.LowLevel.Unsafe; +using Unity.Mathematics; + +namespace Latios.Unsafe +{ + public unsafe struct ThreadStackAllocator : IDisposable + { + #region API + public static int maxAllocatorsPerThread + { + get => s_settings.Data.maxDepths; + set => s_settings.Data.maxDepths = value; + } + + public static int defaultBlockSize + { + get => s_settings.Data.defaultBlockSize; + set => s_settings.Data.defaultBlockSize = value; + } + + public static ThreadStackAllocator GetAllocator() + { + return s_states.Data[JobsUtility.ThreadIndex]->CreateAllocator(); + } + + public void Dispose() + { + CheckAllocatorIsValid(); + m_statePtr->DisposeAllocator(this); + } + + public T* Allocate(int count) where T : unmanaged + { + CheckAllocatorIsValid(); + return (T*)m_statePtr->Allocate((ulong)UnsafeUtility.SizeOf(), (ulong)UnsafeUtility.AlignOf(), (ulong)count); + } + + // Slightly faster than getting the allocator globally. + public ThreadStackAllocator CreateChildAllocator() + { + CheckAllocatorIsValid(); + return m_statePtr->CreateAllocator(); + } + #endregion + + #region Implementation + State* m_statePtr; + int m_firstAllocationIndex; + int m_depth; + + static readonly SharedStatic s_settings = SharedStatic.GetOrCreate(); + static readonly SharedStatic s_states = SharedStatic.GetOrCreate(); + + [StructLayout(LayoutKind.Sequential, Size = JobsUtility.CacheLineSize)] + struct State + { + struct Block + { + public byte* ptr; + public ulong size; + } + + struct Allocation + { + public ulong byteOffset; + public ulong byteCount; + public int blockIndex; + } + + UnsafeList m_blocks; + UnsafeList m_allocations; + State* m_selfPtr; + int m_allocatorDepth; + + public ThreadStackAllocator CreateAllocator() + { + CheckForDepthLeaks(m_allocatorDepth + 1); + m_allocatorDepth++; + return new ThreadStackAllocator + { + m_depth = m_allocatorDepth, + m_firstAllocationIndex = m_allocations.Length, + m_statePtr = m_selfPtr + }; + } + + public byte* Allocate(ulong sizeOfElement, ulong alignOfElement, ulong numElements) + { + var neededBytes = numElements * sizeOfElement; + int nextBlockIndex = 0; + if (!m_allocations.IsEmpty) + { + var lastAllocation = m_allocations[m_allocations.Length - 1]; + var bytesUsedInBlock = lastAllocation.byteOffset + lastAllocation.byteCount; + var block = m_blocks[lastAllocation.blockIndex]; + var bytesUsedInBlockAfterAlignment = CollectionHelper.Align(bytesUsedInBlock, alignOfElement); + var freeBytes = block.size - bytesUsedInBlockAfterAlignment; + if (neededBytes <= freeBytes) + { + m_allocations.Add(new Allocation + { + blockIndex = lastAllocation.blockIndex, + byteCount = neededBytes, + byteOffset = bytesUsedInBlockAfterAlignment + }); + return block.ptr + bytesUsedInBlockAfterAlignment; + } + nextBlockIndex = lastAllocation.blockIndex + 1; + } + if (m_blocks.Length > nextBlockIndex) + { + // We have at least one completely empty block to try and allocate into. + var nextBlock = m_blocks[nextBlockIndex]; + if (nextBlock.size < neededBytes) + { + // The next free block is too small. Destroy all the free blocks and let the new block allocator continue. + for (int i = nextBlockIndex; i < m_blocks.Length; i++) + { + UnsafeUtility.FreeTracked(m_blocks[i].ptr, Allocator.Persistent); + } + m_blocks.Length = nextBlockIndex; + } + else + { + m_allocations.Add(new Allocation + { + blockIndex = nextBlockIndex, + byteCount = neededBytes, + byteOffset = 0 + }); + return nextBlock.ptr; + } + } + // At this point, we simply want to allocate a new block at the end of the list. + var bytesRequiredForBlock = math.max(neededBytes, (ulong)s_settings.Data.defaultBlockSize); + var newBlock = new Block + { + ptr = (byte*)UnsafeUtility.MallocTracked((long)bytesRequiredForBlock, (int)alignOfElement, Allocator.Persistent, 0), + size = bytesRequiredForBlock, + }; + m_blocks.Add(newBlock); + m_allocations.Add(new Allocation + { + blockIndex = nextBlockIndex, + byteCount = neededBytes, + byteOffset = 0 + }); + return newBlock.ptr; + } + + public void DisposeAllocator(ThreadStackAllocator allocator) + { + CheckForDepthLeaks(allocator.m_depth, m_allocatorDepth); + m_allocatorDepth = allocator.m_depth - 1; + m_allocations.Length = allocator.m_firstAllocationIndex; + } + + public static void Init(State* state) + { + state->m_blocks = new UnsafeList(4, Allocator.Persistent, NativeArrayOptions.ClearMemory); + state->m_allocations = new UnsafeList(64, Allocator.Persistent, NativeArrayOptions.ClearMemory); + state->m_allocatorDepth = 0; + state->m_selfPtr = state; + } + + public static void Destroy(State* state) + { + for (int i = 0; i < state->m_blocks.Length; i++) + { + UnsafeUtility.FreeTracked(state->m_blocks[i].ptr, Allocator.Persistent); + } + state->m_blocks.Dispose(); + state->m_allocations.Dispose(); + } + } + + struct StateArray + { + struct State16 + { + public State s0, s1, s2, s3, s4, s5, s6, s7, s8, s9, sa, sb, sc, sd, se, sf; + } + + struct State128 + { + public State16 s0, s1, s2, s3, s4, s5, s6, s7; + } + + State128 m_array; + + public State* this[int index] + { + get + { + fixed (State* ptr = &m_array.s0.s0 ) { + return ptr + index; + } + } + } + + public void Init() + { + UnityEngine.Assertions.Assert.AreEqual(128, JobsUtility.MaxJobThreadCount); + for (int i = 0; i < 128; i++) + { + State.Init(this[i]); + } + } + + public void Destroy() + { + for (int i = 0; i < 128; i++) + { + State.Destroy(this[i]); + } + } + } + + struct Settings + { + public int maxDepths; + public int defaultBlockSize; + } + + // Setup and teardown + static bool s_initialized; + +#if UNITY_EDITOR + [UnityEditor.InitializeOnLoadMethod] +#else + [RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.AfterAssembliesLoaded)] +#endif + internal static void Initialize() + { + if (s_initialized) + return; + + // important: this will always be called from a special unload thread (main thread will be blocking on this) + AppDomain.CurrentDomain.DomainUnload += (_, __) => { Shutdown(); }; + + // There is no domain unload in player builds, so we must be sure to shutdown when the process exits. + AppDomain.CurrentDomain.ProcessExit += (_, __) => { Shutdown(); }; + + s_settings.Data = new Settings { defaultBlockSize = 1024 * 1024, maxDepths = 32 }; + s_states.Data.Init(); + s_initialized = true; + } + + static void Shutdown() + { + if (s_initialized) + s_states.Data.Destroy(); + } + + #endregion + + #region Safety + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckAllocatorIsValid() + { + if (m_statePtr == null) + throw new System.InvalidOperationException("ThreadStackAllocator is not initialized."); + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + static void CheckForDepthLeaks(int depth) + { + if (depth > s_settings.Data.maxDepths) + throw new System.InvalidOperationException( + $"Thread has too many ThreadStackAllocators compared to the max threshold of {s_settings.Data.maxDepths}. This may be a sign of a leak caused by allocator instances not being disposed."); + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + static void CheckForDepthLeaks(int depthOfFreed, int depthsTotal) + { + if (depthOfFreed < depthsTotal) + throw new System.InvalidOperationException( + $"A ThreadStackAllocator is being deallocated before {depthsTotal - depthOfFreed} subsequently created ThreadStackAllocators have been disposed. This is a leak caused by allocator instances not being disposed."); + } + #endregion + } +} + diff --git a/Core/Containers/ThreadLocal/ThreadStackAllocator.cs.meta b/Core/Containers/ThreadLocal/ThreadStackAllocator.cs.meta new file mode 100644 index 0000000..2930365 --- /dev/null +++ b/Core/Containers/ThreadLocal/ThreadStackAllocator.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4187975651b36e049885bd49e763f250 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Core/Containers/UnsafeParallelBlockList.cs b/Core/Containers/UnsafeParallelBlockList.cs deleted file mode 100644 index 9cc14ff..0000000 --- a/Core/Containers/UnsafeParallelBlockList.cs +++ /dev/null @@ -1,457 +0,0 @@ -using System.Runtime.InteropServices; -using Unity.Burst; -using Unity.Collections; -using Unity.Collections.LowLevel.Unsafe; -using Unity.Jobs; -using Unity.Jobs.LowLevel.Unsafe; -using Unity.Mathematics; - -namespace Latios.Unsafe -{ - /// - /// An unsafe container which can be written to by multiple threads and multiple jobs. - /// The container is lock-free and items never change addresses once written. - /// Items written in the same thread in the same job will be read back in the same order. - /// This container is type-erased, but all elements are expected to be of the same size. - /// Unlike Unity's Unsafe* containers, it is safe to copy this type by value. - /// Alignment is 1 byte aligned so any pointer must use UnsafeUtility.CopyStructureToPtr - /// and UnsafeUtility.CopyPtrToStructure when operating directly on pointers. - /// - public unsafe struct UnsafeParallelBlockList : INativeDisposable - { - public readonly int m_elementSize; - private readonly int m_blockSize; - private readonly int m_elementsPerBlock; - private AllocatorManager.AllocatorHandle m_allocator; - - [NativeDisableUnsafePtrRestriction] private PerThreadBlockList* m_perThreadBlockLists; - - /// - /// Construct a new UnsafeParallelBlockList using a UnityEngine allocator - /// - /// The size of each element in bytes that will be stored - /// - /// The number of elements stored per native thread index before needing to perform an additional allocation. - /// Higher values may allocate more memory that is left unused. Lower values may perform more allocations. - /// - /// The allocator to use for allocations - public UnsafeParallelBlockList(int elementSize, int elementsPerBlock, AllocatorManager.AllocatorHandle allocator) - { - m_elementSize = elementSize; - m_elementsPerBlock = elementsPerBlock; - m_blockSize = elementSize * elementsPerBlock; - m_allocator = allocator; - - m_perThreadBlockLists = AllocatorManager.Allocate(allocator, JobsUtility.MaxJobThreadCount); - for (int i = 0; i < JobsUtility.MaxJobThreadCount; i++) - { - m_perThreadBlockLists[i].lastByteAddressInBlock = null; - m_perThreadBlockLists[i].nextWriteAddress = null; - m_perThreadBlockLists[i].nextWriteAddress++; - m_perThreadBlockLists[i].elementCount = 0; - } - } - - //The thread index is passed in because otherwise job reflection can't inject it through a pointer. - /// - /// Write an element for a given thread index - /// - /// It is assumed the size of T is the same as what was passed into elementSize during construction - /// The value to write - /// The thread index to use when writing. This should come from [NativeSetThreadIndex]. - public void Write(T value, int threadIndex) where T : unmanaged - { - var blockList = m_perThreadBlockLists + threadIndex; - if (blockList->nextWriteAddress > blockList->lastByteAddressInBlock) - { - if (blockList->elementCount == 0) - { - blockList->blocks = new UnsafeList(8, m_allocator); - } - BlockPtr newBlockPtr = new BlockPtr - { - ptr = AllocatorManager.Allocate(m_allocator, m_blockSize) - }; - blockList->nextWriteAddress = newBlockPtr.ptr; - blockList->lastByteAddressInBlock = newBlockPtr.ptr + m_blockSize - 1; - blockList->blocks.Add(newBlockPtr); - } - - UnsafeUtility.CopyStructureToPtr(ref value, blockList->nextWriteAddress); - blockList->nextWriteAddress += m_elementSize; - blockList->elementCount++; - } - - /// - /// Reserve memory for an element and return the fixed memory address. - /// - /// The thread index to use when allocating. This should come from [NativeSetThreadIndex]. - /// A pointer where an element can be copied to - public void* Allocate(int threadIndex) - { - var blockList = m_perThreadBlockLists + threadIndex; - if (blockList->nextWriteAddress > blockList->lastByteAddressInBlock) - { - if (blockList->elementCount == 0) - { - blockList->blocks = new UnsafeList(8, m_allocator); - } - BlockPtr newBlockPtr = new BlockPtr - { - ptr = AllocatorManager.Allocate(m_allocator, m_blockSize), - }; - blockList->nextWriteAddress = newBlockPtr.ptr; - blockList->lastByteAddressInBlock = newBlockPtr.ptr + m_blockSize - 1; - blockList->blocks.Add(newBlockPtr); - } - - var result = blockList->nextWriteAddress; - blockList->nextWriteAddress += m_elementSize; - blockList->elementCount++; - return result; - } - - /// - /// Count the number of elements. Do this once and cache the result. - /// - /// The number of elements stored - public int Count() - { - int result = 0; - for (int i = 0; i < JobsUtility.MaxJobThreadCount; i++) - { - result += m_perThreadBlockLists[i].elementCount; - } - return result; - } - - /// - /// Returns true if the struct is not in a default uninitialized state. - /// This may report true incorrectly if the memory where this instance - /// exists was left uninitialized rather than cleared. - /// - public bool isCreated => m_perThreadBlockLists != null; - - /// - /// A pointer to an element stored - /// - public struct ElementPtr - { - public byte* ptr; - } - - /// - /// Gets all the pointers for all elements stored. - /// This does not actually traverse the memory but instead calculates memory addresses from metadata, - /// which is often faster, especially for large elements. - /// - /// An array in which the pointers should be stored. Its Length should be equal to Count(). - public void GetElementPtrs(NativeArray ptrs) - { - int dst = 0; - - for (int threadBlockId = 0; threadBlockId < JobsUtility.MaxJobThreadCount; threadBlockId++) - { - var blockList = m_perThreadBlockLists + threadBlockId; - if (blockList->elementCount > 0) - { - int src = 0; - for (int blockId = 0; blockId < blockList->blocks.Length - 1; blockId++) - { - var address = blockList->blocks[blockId].ptr; - for (int i = 0; i < m_elementsPerBlock; i++) - { - ptrs[dst] = new ElementPtr { ptr = address }; - address += m_elementSize; - src++; - dst++; - } - } - { - var address = blockList->blocks[blockList->blocks.Length - 1].ptr; - for (int i = src; i < blockList->elementCount; i++) - { - ptrs[dst] = new ElementPtr { ptr = address }; - address += m_elementSize; - dst++; - } - } - } - } - } - - /// - /// Copies all the elements stored into values. - /// - /// It is assumed the size of T is the same as what was passed into elementSize during construction - /// An array where the elements should be copied to. Its Length should be equal to Count(). - [Unity.Burst.CompilerServices.IgnoreWarning(1371)] - public void GetElementValues(NativeArray values) where T : struct - { - int dst = 0; - - for (int threadBlockId = 0; threadBlockId < JobsUtility.MaxJobThreadCount; threadBlockId++) - { - var blockList = m_perThreadBlockLists + threadBlockId; - if (blockList->elementCount > 0) - { - int src = 0; - CheckBlockCountMatchesCount(blockList->elementCount, blockList->blocks.Length); - for (int blockId = 0; blockId < blockList->blocks.Length - 1; blockId++) - { - var address = blockList->blocks[blockId].ptr; - for (int i = 0; i < m_elementsPerBlock; i++) - { - UnsafeUtility.CopyPtrToStructure(address, out T temp); - values[dst] = temp; - address += m_elementSize; - src++; - dst++; - } - } - { - var address = blockList->blocks[blockList->blocks.Length - 1].ptr; - for (int i = src; i < blockList->elementCount; i++) - { - UnsafeUtility.CopyPtrToStructure(address, out T temp); - values[dst] = temp; - address += m_elementSize; - dst++; - } - } - } - } - } - - /// - /// Copies all the elements from the blocklists into the contiguous memory region beginning at ptr. - /// - /// The first address of a contiguous memory region large enough to store all values in the blocklists - [Unity.Burst.CompilerServices.IgnoreWarning(1371)] - public void CopyElementsRaw(void* dstPtr) - { - byte* dst = (byte*)dstPtr; - - for (int threadBlockId = 0; threadBlockId < JobsUtility.MaxJobThreadCount; threadBlockId++) - { - var blockList = m_perThreadBlockLists + threadBlockId; - if (blockList->elementCount > 0) - { - int src = 0; - CheckBlockCountMatchesCount(blockList->elementCount, blockList->blocks.Length); - for (int blockId = 0; blockId < blockList->blocks.Length - 1; blockId++) - { - var address = blockList->blocks[blockId].ptr; - UnsafeUtility.MemCpy(dst, address, m_blockSize); - dst += m_blockSize; - src += m_elementsPerBlock; - } - { - var address = blockList->blocks[blockList->blocks.Length - 1].ptr; - if (src < blockList->elementCount) - UnsafeUtility.MemCpy(dst, address, (blockList->elementCount - src) * m_elementSize); - } - } - } - } - - //This catches race conditions if I accidentally pass in 0 for thread index in the parallel writer because copy and paste. - [BurstDiscard] - void CheckBlockCountMatchesCount(int count, int blockCount) - { - int expectedBlocks = count / m_elementsPerBlock; - if (count % m_elementsPerBlock > 0) - expectedBlocks++; - if (blockCount != expectedBlocks) - throw new System.InvalidOperationException($"Block count: {blockCount} does not match element count: {count}"); - } - - [BurstCompile] - struct DisposeJob : IJob - { - public UnsafeParallelBlockList upbl; - - public void Execute() - { - upbl.Dispose(); - } - } - - /// - /// Uses a job to dispose this container - /// - /// A JobHandle for all jobs which should finish before disposal. - /// A JobHandle for the disposal job. - public JobHandle Dispose(JobHandle inputDeps) - { - var jh = new DisposeJob { upbl = this }.Schedule(inputDeps); - m_perThreadBlockLists = null; - return jh; - } - - /// - /// Disposes the container immediately. It is legal to call this from within a job, - /// as long as no other jobs or threads are using it. - /// - public void Dispose() - { - for (int i = 0; i < JobsUtility.MaxJobThreadCount; i++) - { - if (m_perThreadBlockLists[i].elementCount > 0) - { - for (int j = 0; j < m_perThreadBlockLists[i].blocks.Length; j++) - { - var block = m_perThreadBlockLists[i].blocks[j]; - AllocatorManager.Free(m_allocator, block.ptr, m_blockSize); - } - m_perThreadBlockLists[i].blocks.Dispose(); - } - } - AllocatorManager.Free(m_allocator, m_perThreadBlockLists, JobsUtility.MaxJobThreadCount); - } - - private struct BlockPtr - { - public byte* ptr; - } - - [StructLayout(LayoutKind.Sequential, Size = 64)] - private struct PerThreadBlockList - { - public UnsafeList blocks; - public byte* nextWriteAddress; - public byte* lastByteAddressInBlock; - public int elementCount; - } - - /// - /// Gets an enumerator for one of the thread indices in the job. - /// - /// - /// The thread index that was used when the elements were written. - /// This does not have to be the thread index of the reader. - /// In fact, you usually want to iterate through all threads. - /// - /// - public Enumerator GetEnumerator(int nativeThreadIndex) - { - return new Enumerator(m_perThreadBlockLists + nativeThreadIndex, m_elementSize, m_elementsPerBlock); - } - - /// - /// An enumerator which can be used for iterating over the elements written by a single thread index. - /// It is allowed to have multiple enumerators for the same thread index. - /// - public struct Enumerator - { - private PerThreadBlockList* m_perThreadBlockList; - private byte* m_readAddress; - private byte* m_lastByteAddressInBlock; - private int m_blockIndex; - private int m_elementSize; - private int m_elementsPerBlock; - - internal Enumerator(void* perThreadBlockList, int elementSize, int elementsPerBlock) - { - m_perThreadBlockList = (PerThreadBlockList*)perThreadBlockList; - m_readAddress = null; - m_readAddress++; - m_lastByteAddressInBlock = null; - m_blockIndex = -1; - m_elementSize = elementSize; - m_elementsPerBlock = elementsPerBlock; - } - - /// - /// Advance to the next element - /// - /// Returns false if the previous element was the last, true otherwise - public bool MoveNext() - { - m_readAddress += m_elementSize; - if (m_readAddress > m_lastByteAddressInBlock) - { - m_blockIndex++; - if (m_perThreadBlockList->elementCount == 0 || m_blockIndex >= m_perThreadBlockList->blocks.Length) - return false; - - int elementsInBlock = math.min(m_elementsPerBlock, m_perThreadBlockList->elementCount - m_blockIndex * m_elementsPerBlock); - var blocks = m_perThreadBlockList->blocks.Ptr; - m_lastByteAddressInBlock = blocks[m_blockIndex].ptr + elementsInBlock * m_elementSize - 1; - m_readAddress = blocks[m_blockIndex].ptr; - } - return true; - } - - /// - /// Retrieves the current element, copying it to a variable of the specified type. - /// - /// It is assumed the size of T is the same as what was passed into elementSize during construction - /// A value containing a copy of the element stored, reinterpreted with the strong type - public T GetCurrent() where T : struct - { - UnsafeUtility.CopyPtrToStructure(m_readAddress, out T t); - return t; - } - - internal Enumerator GetNextThreadEnumerator() - { - return new Enumerator(m_perThreadBlockList + 1, m_elementSize, m_elementsPerBlock); - } - } - - public AllThreadsEnumerator GetEnumerator() - { - return new AllThreadsEnumerator(new Enumerator(m_perThreadBlockLists, m_elementSize, m_elementsPerBlock)); - } - - public struct AllThreadsEnumerator - { - Enumerator m_enumerator; - int m_threadIndex; - - internal AllThreadsEnumerator(Enumerator thread0Enumerator) - { - m_enumerator = thread0Enumerator; - m_threadIndex = 0; - } - - public bool MoveNext() - { - while (!m_enumerator.MoveNext()) - { - m_threadIndex++; - if (m_threadIndex >= JobsUtility.MaxJobThreadCount) - return false; - m_enumerator = m_enumerator.GetNextThreadEnumerator(); - } - return true; - } - - public T GetCurrent() where T : struct => m_enumerator.GetCurrent(); - } - } - - // Schedule for 128 iterations - //[BurstCompile] - //struct ExampleReadJob : IJobFor - //{ - // [NativeDisableUnsafePtrRestriction] public UnsafeParallelBlockList blockList; - // - // public void Execute(int index) - // { - // var enumerator = blockList.GetEnumerator(index); - // - // while (enumerator.MoveNext()) - // { - // int number = enumerator.GetCurrent(); - // - // if (number == 381) - // UnityEngine.Debug.Log("You found me!"); - // else if (number == 380) - // UnityEngine.Debug.Log("Where?"); - // } - // } - //} -} - diff --git a/Core/Internal/InstantiateCommandBufferUntyped.cs b/Core/Internal/InstantiateCommandBufferUntyped.cs index b483a55..ae470ad 100644 --- a/Core/Internal/InstantiateCommandBufferUntyped.cs +++ b/Core/Internal/InstantiateCommandBufferUntyped.cs @@ -302,7 +302,7 @@ public static void Playback(InstantiateCommandBufferUntyped* icb, EntityManager* var chunkRanges = new NativeList(Allocator.Temp); var chunks = new NativeList(Allocator.Temp); var indicesInChunks = new NativeList(Allocator.Temp); - var componentDataPtrs = new NativeList(Allocator.Temp); + var componentDataPtrs = new NativeList(Allocator.Temp); em->CompleteAllTrackedJobs(); var job0 = new InstantiateAndBuildListsJob @@ -345,10 +345,10 @@ private struct InstantiateAndBuildListsJob [ReadOnly] public InstantiateCommandBufferUntyped icb; public EntityManager em; - public NativeList chunks; - public NativeList chunkRanges; - public NativeList indicesInChunks; - public NativeList componentDataPtrs; + public NativeList chunks; + public NativeList chunkRanges; + public NativeList indicesInChunks; + public NativeList componentDataPtrs; public void Execute() { @@ -357,14 +357,14 @@ public void Execute() var prefabSortkeyArray = new NativeArray(count, Allocator.Temp, NativeArrayOptions.UninitializedMemory); icb.m_prefabSortkeyBlockList->GetElementValues(prefabSortkeyArray); //Step 2: Get the componentData pointers - var unsortedComponentDataPtrs = new NativeArray(count, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var unsortedComponentDataPtrs = new NativeArray(count, Allocator.Temp, NativeArrayOptions.UninitializedMemory); icb.m_componentDataBlockList->GetElementPtrs(unsortedComponentDataPtrs); //Step 3: Sort the arrays by sort key and collapse unique entities var ranks = new NativeArray(count, Allocator.Temp, NativeArrayOptions.UninitializedMemory); RadixSort.RankSortInt3(ranks, prefabSortkeyArray); var sortedPrefabs = new NativeList(count, Allocator.Temp); var sortedPrefabCounts = new NativeList(count, Allocator.Temp); - var sortedComponentDataPtrs = new NativeArray(count, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var sortedComponentDataPtrs = new NativeArray(count, Allocator.Temp, NativeArrayOptions.UninitializedMemory); Entity lastEntity = Entity.Null; for (int i = 0; i < count; i++) { @@ -455,17 +455,17 @@ public int3 GetKey3() private struct WriteComponentDataJob { - [ReadOnly] public InstantiateCommandBufferUntyped icb; - [ReadOnly] public NativeArray chunks; - [ReadOnly] public NativeArray chunkRanges; - [ReadOnly] public NativeArray indicesInChunks; - [ReadOnly] public NativeArray componentDataPtrs; - [ReadOnly] public EntityTypeHandle entityHandle; - public DynamicComponentTypeHandle t0; - public DynamicComponentTypeHandle t1; - public DynamicComponentTypeHandle t2; - public DynamicComponentTypeHandle t3; - public DynamicComponentTypeHandle t4; + [ReadOnly] public InstantiateCommandBufferUntyped icb; + [ReadOnly] public NativeArray chunks; + [ReadOnly] public NativeArray chunkRanges; + [ReadOnly] public NativeArray indicesInChunks; + [ReadOnly] public NativeArray componentDataPtrs; + [ReadOnly] public EntityTypeHandle entityHandle; + public DynamicComponentTypeHandle t0; + public DynamicComponentTypeHandle t1; + public DynamicComponentTypeHandle t2; + public DynamicComponentTypeHandle t3; + public DynamicComponentTypeHandle t4; public void Execute(int i) { @@ -483,7 +483,7 @@ public void Execute(int i) } } - void DoT0(ArchetypeChunk chunk, NativeArray indices, NativeArray dataPtrs) + void DoT0(ArchetypeChunk chunk, NativeArray indices, NativeArray dataPtrs) { var entities = chunk.GetNativeArray(entityHandle); var t0Size = icb.m_state->typesSizes[0]; @@ -497,7 +497,7 @@ void DoT0(ArchetypeChunk chunk, NativeArray indices, NativeArray indices, NativeArray dataPtrs) + void DoT1(ArchetypeChunk chunk, NativeArray indices, NativeArray dataPtrs) { var entities = chunk.GetNativeArray(entityHandle); var t0Size = icb.m_state->typesSizes[0]; @@ -516,7 +516,7 @@ void DoT1(ArchetypeChunk chunk, NativeArray indices, NativeArray indices, NativeArray dataPtrs) + void DoT2(ArchetypeChunk chunk, NativeArray indices, NativeArray dataPtrs) { var entities = chunk.GetNativeArray(entityHandle); var t0Size = icb.m_state->typesSizes[0]; @@ -541,7 +541,7 @@ void DoT2(ArchetypeChunk chunk, NativeArray indices, NativeArray indices, NativeArray dataPtrs) + void DoT3(ArchetypeChunk chunk, NativeArray indices, NativeArray dataPtrs) { var entities = chunk.GetNativeArray(entityHandle); var t0Size = icb.m_state->typesSizes[0]; @@ -570,7 +570,7 @@ void DoT3(ArchetypeChunk chunk, NativeArray indices, NativeArray indices, NativeArray dataPtrs) + void DoT4(ArchetypeChunk chunk, NativeArray indices, NativeArray dataPtrs) { var entities = chunk.GetNativeArray(entityHandle); var t0Size = icb.m_state->typesSizes[0]; diff --git a/Core/Math/Rng.cs b/Core/Math/Rng.cs index 1196ee6..49dee87 100644 --- a/Core/Math/Rng.cs +++ b/Core/Math/Rng.cs @@ -154,6 +154,30 @@ public void ShuffleElements(T list) where T : unmanaged, INativeList wh } } + public static class SystemRngStateExtensions + { + public static void InitSystemRng(this ref SystemState state, uint seed) + { + state.EntityManager.AddComponentData(state.SystemHandle, new SystemRng(seed)); + } + + public static void InitSystemRng(this ref SystemState state, FixedString128Bytes seedString) + { + state.EntityManager.AddComponentData(state.SystemHandle, new SystemRng(seedString)); + } + + public static SystemRng GetJobRng(this ref SystemState state) + { + return state.EntityManager.GetComponentDataRW(state.SystemHandle).ValueRW.Shuffle(); + } + + public static Rng.RngSequence GetMainThreadRng(this ref SystemState state) + { + var srng = GetJobRng(ref state); + return srng.rng.GetSequence(int.MaxValue); // Do something most people won't encounter in jobs for extra randomness. + } + } + public struct SystemRng : IComponentData { public Rng rng; @@ -238,5 +262,60 @@ public void BeginChunk(int unfilteredChunkIndex) public void ShuffleElements(NativeArray array) where T : unmanaged => currentSequence.ShuffleElements(array); public void ShuffleElements(T list) where T : unmanaged, INativeList where U : unmanaged => currentSequence.ShuffleElements(list); } + + // Todo: In order for the below to work, Unity would need to change the source generators to invoke these interface methods + // via interface-constrained generics, rather than calling them directly. Calling them directly doesn't export the symbols + // to the struct type for easy call access. + // An alternative would be to write a source generator that adds the IJobEntityChunkBeginEnd interface and implementation + // if it isn't already present. That could even define the SystemRng property, though we'd probably need to validate + // the SystemRng instance if we went that route. + + // /// + // /// An interface to implement in IJobEntity jobs to automatically set up SystemRng. + // /// You simply need to define a SystemRng autoproperty named "rng" in your job for everything + // /// to function correctly within the job. + // /// + // public interface IJobEntityRng : IJobEntityChunkBeginEnd + // { + // public SystemRng rng { get; set; } + // + // new public bool OnChunkBegin(in ArchetypeChunk chunk, + // int unfilteredChunkIndex, + // bool useEnabledMask, + // in Unity.Burst.Intrinsics.v128 chunkEnabledMask) + // { + // var instance = rng; + // instance.BeginChunk(unfilteredChunkIndex); + // rng = instance; + // return true; + // } + // + // new public void OnChunkEnd(in ArchetypeChunk chunk, + // int unfilteredChunkIndex, + // bool useEnabledMask, + // in Unity.Burst.Intrinsics.v128 chunkEnabledMask, + // bool chunkWasExecuted) + // { + // } + // + // bool IJobEntityChunkBeginEnd.OnChunkBegin(in ArchetypeChunk chunk, + // int unfilteredChunkIndex, + // bool useEnabledMask, + // in Unity.Burst.Intrinsics.v128 chunkEnabledMask) + // { + // var instance = rng; + // instance.BeginChunk(unfilteredChunkIndex); + // rng = instance; + // return true; + // } + // + // void IJobEntityChunkBeginEnd.OnChunkEnd(in ArchetypeChunk chunk, + // int unfilteredChunkIndex, + // bool useEnabledMask, + // in Unity.Burst.Intrinsics.v128 chunkEnabledMask, + // bool chunkWasExecuted) + // { + // } + // } } diff --git a/Core/Math/mathExtensions.cs b/Core/Math/mathExtensions.cs index ed2d30e..5b9a700 100644 --- a/Core/Math/mathExtensions.cs +++ b/Core/Math/mathExtensions.cs @@ -3,11 +3,20 @@ public static class MathExtensions { /// - /// Extends a 3-component vector to a 4 component vector, assigning 1f to the w component + /// Extends a 3-component vector to a 4-component vector, assigning 1f to the w component /// public static float4 xyz1(this float3 value) { return new float4(value, 1f); } + + /// + /// Extends a 2-component vector to a 3-component vector, putting the y in the 2D vector into the z + /// of the 3D vector and zeroing the y in the 3D vector. + /// + public static float3 x0y(this float2 value) + { + return new float3(value.x, 0f, value.y); + } } diff --git a/Core/Systems/Scenes/SceneManagerSystem.cs b/Core/Systems/Scenes/SceneManagerSystem.cs index 3a00b26..9907280 100644 --- a/Core/Systems/Scenes/SceneManagerSystem.cs +++ b/Core/Systems/Scenes/SceneManagerSystem.cs @@ -24,8 +24,11 @@ public partial class SceneManagerSystem : SubSystem private EntityQuery m_rlsQuery; private EntityQuery m_unitySubsceneLoadQuery; private EntityQuery m_dontDestroyOnSceneChangeQuery; + private EntityQuery m_newSubsceneReferenceQuery; private bool m_paused = false; + private struct ObservedSubsceneReferenceTag : IComponentData { } + protected override void OnCreate() { CurrentScene curr = new CurrentScene @@ -36,8 +39,9 @@ protected override void OnCreate() }; worldBlackboardEntity.AddComponentData(curr); - m_unitySubsceneLoadQuery = Fluent.With().Build(); + m_unitySubsceneLoadQuery = Fluent.With().Without().Build(); m_dontDestroyOnSceneChangeQuery = Fluent.With().With().IncludeDisabledEntities().IncludePrefabs().Build(); + m_newSubsceneReferenceQuery = Fluent.With().Without().Build(); } protected override void OnUpdate() @@ -100,6 +104,31 @@ protected override void OnUpdate() } worldBlackboardEntity.SetComponentData(currentScene); + if (!m_newSubsceneReferenceQuery.IsEmptyIgnoreFilter) + { + var newSubsceneEntities = m_newSubsceneReferenceQuery.ToEntityArray(Allocator.Temp); + foreach (var e in newSubsceneEntities) + { + var subsceneRef = EntityManager.GetComponentObject(e); + if (subsceneRef.TryGetComponent(out var options)) + { + if (options.loadOptions == SubsceneLoadOptions.LoadOptions.Asynchronous) + { + EntityManager.AddComponent(e); + if (EntityManager.HasComponent(e)) + { + foreach (var s in EntityManager.GetBuffer(e)) + EntityManager.AddComponent(s.SectionEntity); + } + } + } + } + EntityManager.AddComponent(m_newSubsceneReferenceQuery); + } + + if (m_unitySubsceneLoadQuery.IsEmptyIgnoreFilter) + return; + var subsceneRequests = m_unitySubsceneLoadQuery.ToComponentDataArray(Allocator.Temp); var subsceneEntities = m_unitySubsceneLoadQuery.ToEntityArray(Allocator.Temp); for (int i = 0; i < subsceneEntities.Length; i++) @@ -109,12 +138,21 @@ protected override void OnUpdate() if ((request.LoadFlags & SceneLoadFlags.DisableAutoLoad) == 0) { request.LoadFlags |= SceneLoadFlags.BlockOnStreamIn; - EntityManager.SetComponentData(subsceneEntity, request); if (EntityManager.HasComponent(subsceneEntity)) { foreach (var s in EntityManager.GetBuffer(subsceneEntity)) EntityManager.AddComponentData(s.SectionEntity, request); } + else if (EntityManager.HasComponent(subsceneEntity)) + { + var actualSubscene = EntityManager.GetComponentData(subsceneEntity).SceneEntity; + if (EntityManager.HasComponent(actualSubscene)) + { + EntityManager.AddComponent(subsceneEntity); + continue; + } + } + EntityManager.SetComponentData(subsceneEntity, request); } } } diff --git a/Core/Utilities/CollectionsExtensions.cs b/Core/Utilities/CollectionsExtensions.cs index d05408a..c10f090 100644 --- a/Core/Utilities/CollectionsExtensions.cs +++ b/Core/Utilities/CollectionsExtensions.cs @@ -10,6 +10,13 @@ public static void AddRangeFromBlob(this NativeList list, ref BlobArray for (int i = 0; i < data.Length; i++) list.Add(data[i]); } + + public static NativeList Clone(this NativeList list, AllocatorManager.AllocatorHandle allocator) where T : unmanaged + { + var result = new NativeList(list.Length, allocator); + result.AddRangeNoResize(list); + return result; + } } } diff --git a/EntitiesExposed/ArchetypeChunkExposed.cs b/EntitiesExposed/ArchetypeChunkExposed.cs index ecaecd8..d841a16 100644 --- a/EntitiesExposed/ArchetypeChunkExposed.cs +++ b/EntitiesExposed/ArchetypeChunkExposed.cs @@ -41,6 +41,12 @@ public static unsafe uint GetChunkIndexAsUint(in this ArchetypeChunk chunk) { return Mathematics.math.asuint(chunk.m_Chunk); } + + // Todo: Move to dedicated file if more extensions are needed. + public static unsafe bool UsesEnabledFiltering(in this EntityQuery query) + { + return query._GetImpl()->_QueryData->HasEnableableComponents != 0; + } } } diff --git a/Kinemation/Authoring/BakingSystems/RendererBakingSystem.cs b/Kinemation/Authoring/BakingSystems/RendererBakingSystem.cs index f09b906..79f95bb 100644 --- a/Kinemation/Authoring/BakingSystems/RendererBakingSystem.cs +++ b/Kinemation/Authoring/BakingSystems/RendererBakingSystem.cs @@ -10,6 +10,8 @@ using Hash128 = Unity.Entities.Hash128; using static Unity.Entities.SystemAPI; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; namespace Latios.Kinemation.Authoring.Systems { @@ -94,11 +96,12 @@ protected override void OnUpdate() var renderablesWithLightmapsQuery = QueryBuilder().WithAll() .WithOptions(EntityQueryOptions.IncludePrefab | EntityQueryOptions.IncludeDisabledEntities).Build(); - var meshMap = new NativeHashMap, int>(128, Allocator.TempJob); - var materialMap = new NativeHashMap, int>(128, Allocator.TempJob); - var meshList = new NativeList >(Allocator.TempJob); - var materialList = new NativeList >(Allocator.TempJob); - var rangesList = new NativeList(Allocator.TempJob); + var meshMap = new NativeHashMap, int>(128, Allocator.TempJob); + var materialMap = new NativeHashMap, int>(128, Allocator.TempJob); + var meshList = new NativeList >(Allocator.TempJob); + var materialList = new NativeList >(Allocator.TempJob); + var rangesList = new NativeList(Allocator.TempJob); + var duplicatesMap = new NativeParallelMultiHashMap(128, Allocator.TempJob); if (!renderablesWithLightmapsQuery.IsEmptyIgnoreFilter) { new CollectUniqueMeshesAndMaterialsJob @@ -110,9 +113,12 @@ protected override void OnUpdate() }.Run(renderablesWithLightmapsQuery); new BuildMaterialMeshInfoJob { - materialMap = materialMap, - meshMap = meshMap, - rangesList = rangesList + materialMap = materialMap, + meshMap = meshMap, + rangesList = rangesList, + bufferLookup = SystemAPI.GetBufferLookup(true), + mmiLookup = SystemAPI.GetComponentLookup(), + duplicateRangesFilterMap = duplicatesMap }.Run(renderablesWithLightmapsQuery); var rma = CreateRenderMeshArrayFromRefArrays(meshList.AsArray(), materialList.AsArray(), rangesList.AsArray()); state.EntityManager.SetSharedComponentManaged(renderablesWithLightmapsQuery, rma); @@ -122,6 +128,7 @@ protected override void OnUpdate() materialMap.Clear(); materialList.Clear(); rangesList.Clear(); + duplicatesMap.Clear(); } var renderablesWithoutLightmapsQuery = QueryBuilder().WithAll().WithAbsent() .WithOptions(EntityQueryOptions.IncludePrefab | EntityQueryOptions.IncludeDisabledEntities).Build(); @@ -136,9 +143,12 @@ protected override void OnUpdate() }.Run(renderablesWithoutLightmapsQuery); new BuildMaterialMeshInfoJob { - materialMap = materialMap, - meshMap = meshMap, - rangesList = rangesList + materialMap = materialMap, + meshMap = meshMap, + rangesList = rangesList, + bufferLookup = SystemAPI.GetBufferLookup(true), + mmiLookup = SystemAPI.GetComponentLookup(), + duplicateRangesFilterMap = duplicatesMap }.Run(renderablesWithoutLightmapsQuery); var rma = CreateRenderMeshArrayFromRefArrays(meshList.AsArray(), materialList.AsArray(), rangesList.AsArray()); state.EntityManager.SetSharedComponentManaged(renderablesWithoutLightmapsQuery, rma); @@ -149,6 +159,7 @@ protected override void OnUpdate() materialMap.Dispose(); materialList.Dispose(); rangesList.Dispose(); + duplicatesMap.Dispose(); m_lightmapBakingContext.EndConversion(); } @@ -225,14 +236,42 @@ public void Execute(in DynamicBuffer buffer) } } + struct PossiblyUniqueMMI : IEquatable + { + public UnityObjectRef firstMesh; + public UnityObjectRef lastMesh; + public UnityObjectRef firstMaterial; + public UnityObjectRef lastMaterial; + public int firstSubmesh; + public int lastSubmesh; + public int count; + + public bool Equals(PossiblyUniqueMMI other) + { + return (firstSubmesh == other.firstSubmesh && lastSubmesh == other.lastSubmesh && + count == other.count && firstMesh.Equals(other.firstMesh) && lastMesh.Equals(other.lastMesh) && + firstMaterial.Equals(other.firstMaterial) && lastMaterial.Equals(other.lastMaterial)); + } + + public override int GetHashCode() + { + int4x2 hash = new int4x2(new int4(firstMesh.GetHashCode(), lastMesh.GetHashCode(), firstMaterial.GetHashCode(), lastMesh.GetHashCode()), + new int4(firstSubmesh, lastSubmesh, count, count)); + return hash.GetHashCode(); + } + } + [BurstCompile] partial struct BuildMaterialMeshInfoJob : IJobEntity { - [ReadOnly] public NativeHashMap, int> meshMap; - [ReadOnly] public NativeHashMap, int> materialMap; - public NativeList rangesList; - - public void Execute(ref MaterialMeshInfo mmi, in DynamicBuffer buffer) + [ReadOnly] public NativeHashMap, int> meshMap; + [ReadOnly] public NativeHashMap, int> materialMap; + [ReadOnly] public BufferLookup bufferLookup; + public NativeList rangesList; + public NativeParallelMultiHashMap duplicateRangesFilterMap; + [NativeDisableContainerSafetyRestriction] public ComponentLookup mmiLookup; + + public unsafe void Execute(Entity entity, ref MaterialMeshInfo mmi, in DynamicBuffer buffer) { if (buffer.Length == 1) { @@ -245,11 +284,34 @@ public void Execute(ref MaterialMeshInfo mmi, in DynamicBuffer()); + if (match == 0) + { + mmi = mmiLookup[candidateEntity]; + return; + } + } + // Create MMI from ranges int rangesStartIndex = rangesList.Length; - // Todo: Deduplicate ranges - foreach (var element in buffer) { rangesList.Add(new MaterialMeshIndex @@ -261,6 +323,7 @@ public void Execute(ref MaterialMeshInfo mmi, in DynamicBuffer public BindingMode bindingMode = BindingMode.BakeTime; + /// + /// Enables Dual Quaternion Skinning instead of Linear Blend (Matrix) Skinning. + /// + public bool useDualQuaternionSkinning = false; + /// /// Assign a list of bone name hierarchy paths (leaf to root delineated with '/') corresponding to each bone index in the mesh /// (which also corresponds to the BindPoses array). Each path will be matched to a bone in the skeleton whose skeleton path @@ -41,5 +47,16 @@ public enum BindingMode OverridePaths } } + + public class SkinnedMeshSettingsBaker : Baker + { + public override void Bake(SkinnedMeshSettingsAuthoring authoring) + { + if (authoring.useDualQuaternionSkinning) + { + AddComponent(GetEntity(TransformUsageFlags.Renderable)); + } + } + } } diff --git a/Kinemation/Components/InternalComponents.cs b/Kinemation/Components/InternalComponents.cs index 4c90deb..e87a59e 100644 --- a/Kinemation/Components/InternalComponents.cs +++ b/Kinemation/Components/InternalComponents.cs @@ -389,7 +389,7 @@ internal partial struct ExposedSkeletonBoundsArrays : ICollectionComponent public NativeList batchedAabbs; public NativeList allAabbsPreOffset; public NativeList meshOffsets; - public const int kCountPerBatch = 1 << 32; // Todo: Is there a better size? + public const int kCountPerBatch = 32; // Todo: Is there a better size? public JobHandle TryDispose(JobHandle inputDeps) { @@ -439,6 +439,28 @@ internal partial struct DeformClassificationMap : ICollectionComponent // The data is owned by a world or system rewindable allocator. public JobHandle TryDispose(JobHandle inputDeps) => inputDeps; } + + internal partial struct LODCrossfadePtrMap : ICollectionComponent + { + public unsafe struct CrossfadePtr { public byte* ptr; } + public struct ChunkIdentifier : IEquatable + { + public uint batchID; + public int batchStartIndex; + + bool IEquatable.Equals(ChunkIdentifier other) + { + return batchID.Equals(other.batchID) && batchStartIndex.Equals(other.batchStartIndex); + } + + public override int GetHashCode() => new int2(math.asint(batchID), batchStartIndex).GetHashCode(); + } + + public NativeHashMap chunkIdentifierToPtrMap; + + // The data is owned by a world or system rewindable allocator. + public JobHandle TryDispose(JobHandle inputDeps) => inputDeps; + } #endregion } diff --git a/Kinemation/Components/MeshComponents.cs b/Kinemation/Components/MeshComponents.cs index 4abd4c2..315e283 100644 --- a/Kinemation/Components/MeshComponents.cs +++ b/Kinemation/Components/MeshComponents.cs @@ -8,6 +8,29 @@ namespace Latios.Kinemation { #region All Meshes + public struct LodCrossfade : IComponentData + { + public byte raw; + + public void SetOpacity(float opacity, bool isComplementary) + { + int snorm = (int)math.round(opacity * 127f); + snorm = math.select(snorm, -snorm, isComplementary); + snorm &= 0xff; + raw = (byte)snorm; + } + + public float opacity + { + get + { + int reg = raw; + var pos = math.select(reg, 256 - reg, reg > 128); + return pos / 127f; + } + } + } + /// /// An optional component that when present will be enabled for the duration of the frame /// following a frame it was rendered by some view (including shadows), and disabled otherwise. @@ -147,7 +170,7 @@ public struct FailedBindingsRootTag : IComponentData { } /// Usage: Add to an entity to make it use Dual Quaternion Skinning. Remove to return /// to matrix skinning. /// - internal struct DualQuaternionSkinningDeformTag : IComponentData { } + public struct DualQuaternionSkinningDeformTag : IComponentData { } #endregion diff --git a/Kinemation/Components/SkeletonComponents.cs b/Kinemation/Components/SkeletonComponents.cs index 1d699a9..f6987e3 100644 --- a/Kinemation/Components/SkeletonComponents.cs +++ b/Kinemation/Components/SkeletonComponents.cs @@ -73,7 +73,7 @@ public struct SkeletonBindingPathsBlob /// The index into pathsInReversedNotation, which is also a boneIndex /// The string to search /// Returns true if the string matches the start - public unsafe bool StartsWith(int pathIndex, in FixedString64Bytes searchString) + public unsafe bool StartsWith(int pathIndex, in T searchString) where T : unmanaged, INativeList, IUTF8Bytes { if (searchString.Length > pathsInReversedNotation[pathIndex].Length) return false; @@ -87,7 +87,7 @@ public unsafe bool StartsWith(int pathIndex, in FixedString64Bytes searchString) /// The string to search /// The first path index that began with the search string. This index corresponds to a bone index. /// Returns true if a match was found - public bool TryGetFirstPathIndexThatStartsWith(in FixedString64Bytes searchString, out int foundPathIndex) + public bool TryGetFirstPathIndexThatStartsWith(in T searchString, out int foundPathIndex) where T : unmanaged, INativeList, IUTF8Bytes { for (foundPathIndex = 0; foundPathIndex < pathsInReversedNotation.Length; foundPathIndex++) { diff --git a/Kinemation/Editor/ExtraShaderGraphNodes/LatiosVertexSkinningNode.cs b/Kinemation/Editor/ExtraShaderGraphNodes/LatiosVertexSkinningNode.cs index af49f7e..a6f0472 100644 --- a/Kinemation/Editor/ExtraShaderGraphNodes/LatiosVertexSkinningNode.cs +++ b/Kinemation/Editor/ExtraShaderGraphNodes/LatiosVertexSkinningNode.cs @@ -31,7 +31,7 @@ class LatiosVertexSkinningNode : AbstractMaterialNode, IGeneratesBodyCode, IGene public enum Algorithm { Matrix = 0, - //DualQuaternion = 1 + DualQuaternion = 1 } public enum Source diff --git a/Kinemation/Resources/BatchSkinning.compute b/Kinemation/Resources/BatchSkinning.compute index 4e504ba..cffbf2d 100644 --- a/Kinemation/Resources/BatchSkinning.compute +++ b/Kinemation/Resources/BatchSkinning.compute @@ -97,7 +97,7 @@ uniform RWByteAddressBuffer _dstTransforms : register(u2); // However, we pad out DQ + float3 to 12 floats to help GPUs // with larger than 32-bit bank accesses. // So we always end up with a TransformUnion of 682 -groupshared TransformUnion gs_transforms[682]; +groupshared TransformUnion gs_transforms[682]; uint _startOffset; @@ -269,7 +269,7 @@ float3x4 mul3x4(float3x4 a, float3x4 b) void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) { const uint groupId = groupIds.x; - const uint4 skeletonCommands = _metaBuffer.Load4((groupId + _startOffset) * 16 ); + const uint4 skeletonCommands = _metaBuffer.Load4((groupId + _startOffset) * 16); const uint skeletonCount = skeletonCommands.x & 0xffff; const uint skeletonOperation = skeletonCommands.x >> 16; @@ -694,12 +694,12 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) { // We write the bindpose in the second part of groupshared gs_transforms[meshBonesCount + i] = _bindPoses[meshBindposesStart + i]; - + Qvvs boneQvvs = _skeletonQvvsTransforms[skeletonTransformStartIndex + i]; Dqs boneDqs = qvvsToDqs(boneQvvs); // We write the world DQS in the first part of groupshared. gs_transforms[i] = dqsToTransformUnionDqs(boneDqs); - + } GroupMemoryBarrierWithGroupSync(); } @@ -836,7 +836,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) Dqs dqs = transformUnionDqsToDqs(gs_transforms[skeletonOffsetInGs + boneIndex]); if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + weight = -weight; bindposeReal += dqs.r * weight; bindposeDual += dqs.d * weight; localScale += dqs.scale.xyz * weight; @@ -852,7 +852,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) bpQvvs.rotation = bindposeReal; bindposeReal.xyz = -bindposeReal.xyz; bpQvvs.position.xyz = mulQuatQuat(2 * bindposeDual, bindposeReal).xyz; - bpQvvs.stretchScale = float4(0, 0, 0, 1); + bpQvvs.stretchScale = float4(1, 1, 1, 1); float3x4 deform = qvvsToMatrix(bpQvvs); float3x4 scale = float3x4( @@ -863,7 +863,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) deform = mul3x4(scale, deform); // This forces the load to be deferred until after deform is computed, saving VGPRs - uint cancel = any(deform != 0) ? 1 : 0; + uint cancel = any(deform) ? 1 : 0; Vertex dstVertex = (Vertex)0; Vertex vertex = _srcVertices[meshVerticesStart + vertexIndexBase + threadId * cancel + inner * THREAD_GROUP_SIZE]; @@ -927,7 +927,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) Dqs dqs = transformUnionDqsToDqs(gs_transforms[skeletonOffsetInGs + boneIndex]); if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + weight = -weight; bindposeReal += dqs.r * weight; bindposeDual += dqs.d * weight; localScale += dqs.scale.xyz * weight; @@ -943,18 +943,18 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) bpQvvs.rotation = bindposeReal; bindposeReal.xyz = -bindposeReal.xyz; bpQvvs.position.xyz = mulQuatQuat(2 * bindposeDual, bindposeReal).xyz; - bpQvvs.stretchScale = float4(0, 0, 0, 1); + bpQvvs.stretchScale = float4(1, 1, 1, 1); float3x4 deform = qvvsToMatrix(bpQvvs); float3x4 scale = float3x4( - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0 + localScale.x, 0, 0, 0, + 0, localScale.y, 0, 0, + 0, 0, localScale.z, 0 ); deform = mul3x4(scale, deform); // This forces the load to be deferred until after deform is computed, saving VGPRs - uint cancel = any(deform != 0) ? 1 : 0; + uint cancel = any(deform) ? 1 : 0; Vertex dstVertex = (Vertex)0; Vertex vertex = _srcVertices[meshVerticesStart + vertexIndexBase + threadId * cancel + inner * THREAD_GROUP_SIZE]; @@ -1031,7 +1031,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) Dqs dqs = transformUnionDqsToDqs(gs_transforms[skeletonOffsetInGs + boneIndex]); if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + weight = -weight; bindposeReal += dqs.r * weight; bindposeDual += dqs.d * weight; localScale += dqs.scale.xyz * weight; @@ -1047,7 +1047,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) bpQvvs.rotation = bindposeReal; bindposeReal.xyz = -bindposeReal.xyz; bpQvvs.position.xyz = mulQuatQuat(2 * bindposeDual, bindposeReal).xyz; - bpQvvs.stretchScale = float4(0, 0, 0, 1); + bpQvvs.stretchScale = float4(1, 1, 1, 1); float3x4 deform = qvvsToMatrix(bpQvvs); float3x4 scale = float3x4( @@ -1058,7 +1058,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) deform = mul3x4(scale, deform); // This forces the load to be deferred until after deform is computed, saving VGPRs - uint cancel = any(deform != 0) ? 1 : 0; + uint cancel = any(deform) ? 1 : 0; Vertex dstVertex = (Vertex)0; Vertex vertex = _dstVertices[dstStart + vertexIndexBase + threadId * cancel + inner * THREAD_GROUP_SIZE]; @@ -1122,7 +1122,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) Dqs dqs = transformUnionDqsToDqs(gs_transforms[skeletonOffsetInGs + boneIndex]); if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + weight = -weight; bindposeReal += dqs.r * weight; bindposeDual += dqs.d * weight; localScale += dqs.scale.xyz * weight; @@ -1138,18 +1138,18 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) bpQvvs.rotation = bindposeReal; bindposeReal.xyz = -bindposeReal.xyz; bpQvvs.position.xyz = mulQuatQuat(2 * bindposeDual, bindposeReal).xyz; - bpQvvs.stretchScale = float4(0, 0, 0, 1); + bpQvvs.stretchScale = float4(1, 1, 1, 1); float3x4 deform = qvvsToMatrix(bpQvvs); float3x4 scale = float3x4( - 1, 0, 0, 0, - 0, 1, 0, 0, - 0, 0, 1, 0 + localScale.x, 0, 0, 0, + 0, localScale.y, 0, 0, + 0, 0, localScale.z, 0 ); deform = mul3x4(scale, deform); // This forces the load to be deferred until after deform is computed, saving VGPRs - uint cancel = any(deform != 0) ? 1 : 0; + uint cancel = any(deform) ? 1 : 0; Vertex dstVertex = (Vertex)0; Vertex vertex = _dstVertices[dstStart + vertexIndexBase + threadId * cancel + inner * THREAD_GROUP_SIZE]; @@ -1226,7 +1226,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) Dqs dqs = transformUnionDqsToDqs(gs_transforms[skeletonOffsetInGs + boneIndex]); localScale += dqs.scale.xyz * weight; if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + weight = -weight; worldReal += dqs.r * weight; worldDual += dqs.d * weight; } @@ -1245,7 +1245,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) float3x4 deform = qvvsToMatrix(worldQvvs); // This forces the load to be deferred until after deform is computed, saving VGPRs - uint cancel = any(deform != 0) ? 1 : 0; + uint cancel = any(deform) ? 1 : 0; Vertex dstVertex = (Vertex)0; Vertex vertex = _dstVertices[dstStart + vertexIndexBase + threadId * cancel + inner * THREAD_GROUP_SIZE]; @@ -1311,7 +1311,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) Dqs dqs = transformUnionDqsToDqs(gs_transforms[skeletonOffsetInGs + boneIndex]); localScale += dqs.scale.xyz * weight; if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + weight = -weight; worldReal += dqs.r * weight; worldDual += dqs.d * weight; } @@ -1330,7 +1330,7 @@ void BatchSkinning(uint threadId : SV_GroupIndex, uint3 groupIds : SV_GroupID) float3x4 deform = qvvsToMatrix(worldQvvs); // This forces the load to be deferred until after deform is computed, saving VGPRs - uint cancel = any(deform != 0) ? 1 : 0; + uint cancel = any(deform) ? 1 : 0; Vertex dstVertex = (Vertex)0; Vertex vertex = _dstVertices[dstStart + vertexIndexBase + threadId * cancel + inner * THREAD_GROUP_SIZE]; diff --git a/Kinemation/ShaderLibrary/VertexSkinning.hlsl b/Kinemation/ShaderLibrary/VertexSkinning.hlsl index 052877e..8faa002 100644 --- a/Kinemation/ShaderLibrary/VertexSkinning.hlsl +++ b/Kinemation/ShaderLibrary/VertexSkinning.hlsl @@ -189,29 +189,29 @@ void vertexSkinDqs(uint4 boneIndices, float4 boneWeights, uint2 skeletonBase, in if (boneWeights.y > 0.0) { dqs = transformUnionDqsToDqs(_latiosBindPoses[skeletonBase.y + boneIndices.y]); + localScale += dqs.scale.xyz * boneWeights.y; if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + boneWeights.y = -boneWeights.y; bindposeReal += dqs.r * boneWeights.y; bindposeDual += dqs.d * boneWeights.y; - localScale += dqs.scale.xyz * boneWeights.y; } if (boneWeights.z > 0.0) { dqs = transformUnionDqsToDqs(_latiosBindPoses[skeletonBase.y + boneIndices.z]); + localScale += dqs.scale.xyz * boneWeights.z; if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + boneWeights.z = -boneWeights.z; bindposeReal += dqs.r * boneWeights.z; bindposeDual += dqs.d * boneWeights.z; - localScale += dqs.scale.xyz * boneWeights.z; } if (boneWeights.w > 0.0) { dqs = transformUnionDqsToDqs(_latiosBindPoses[skeletonBase.y + boneIndices.w]); + localScale += dqs.scale.xyz * boneWeights.w; if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + boneWeights.w = -boneWeights.w; bindposeReal += dqs.r * boneWeights.w; bindposeDual += dqs.d * boneWeights.w; - localScale += dqs.scale.xyz * boneWeights.w; } { @@ -224,7 +224,7 @@ void vertexSkinDqs(uint4 boneIndices, float4 boneWeights, uint2 skeletonBase, in bpQvvs.rotation = bindposeReal; bindposeReal.xyz = -bindposeReal.xyz; bpQvvs.position.xyz = mulQuatQuat(2 * bindposeDual, bindposeReal).xyz; - bpQvvs.stretchScale = float4(0, 0, 0, 1); + bpQvvs.stretchScale = float4(1, 1, 1, 1); float3x4 deform = qvvsToMatrix(bpQvvs); float3x4 scale = float3x4( @@ -250,29 +250,29 @@ void vertexSkinDqs(uint4 boneIndices, float4 boneWeights, uint2 skeletonBase, in if (boneWeights.y > 0.0) { dqs = transformUnionDqsToDqs(readBone(skeletonBase.x + boneIndices.y)); + localScale += dqs.scale.xyz * boneWeights.y; if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + boneWeights.y = -boneWeights.y; worldReal += dqs.r * boneWeights.y; worldDual += dqs.d * boneWeights.y; - localScale += dqs.scale.xyz * boneWeights.y; } if (boneWeights.z > 0.0) { dqs = transformUnionDqsToDqs(readBone(skeletonBase.x + boneIndices.z)); + localScale += dqs.scale.xyz * boneWeights.z; if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + boneWeights.z = -boneWeights.z; worldReal += dqs.r * boneWeights.z; worldDual += dqs.d * boneWeights.z; - localScale += dqs.scale.xyz * boneWeights.z; } if (boneWeights.w > 0.0) { dqs = transformUnionDqsToDqs(readBone(skeletonBase.x + boneIndices.w)); + localScale += dqs.scale.xyz * boneWeights.w; if (dot(dqs.r, firstBoneRot) < 0) - dqs.r.w = -dqs.r.w; + boneWeights.w = -boneWeights.w; worldReal += dqs.r * boneWeights.w; worldDual += dqs.d * boneWeights.w; - localScale += dqs.scale.xyz * boneWeights.w; } { diff --git a/Kinemation/Systems/Culling/GenerateBrgDrawCommandsSystem.cs b/Kinemation/Systems/Culling/GenerateBrgDrawCommandsSystem.cs index cf9ed61..970fbe2 100644 --- a/Kinemation/Systems/Culling/GenerateBrgDrawCommandsSystem.cs +++ b/Kinemation/Systems/Culling/GenerateBrgDrawCommandsSystem.cs @@ -49,8 +49,9 @@ public void OnCreate(ref SystemState state) [BurstCompile] public unsafe void OnUpdate(ref SystemState state) { - var brgCullingContext = latiosWorld.worldBlackboardEntity.GetCollectionComponent(); - var cullingContext = latiosWorld.worldBlackboardEntity.GetComponentData(); + var brgCullingContext = latiosWorld.worldBlackboardEntity.GetCollectionComponent(); + var lodCrossfadePtrMap = latiosWorld.worldBlackboardEntity.GetCollectionComponent(true); + var cullingContext = latiosWorld.worldBlackboardEntity.GetComponentData(); var chunkList = new NativeList(m_metaQuery.CalculateEntityCountWithoutFiltering(), state.WorldUpdateAllocator); @@ -83,6 +84,7 @@ public unsafe void OnUpdate(ref SystemState state) EntitiesGraphicsChunkInfo = GetComponentTypeHandle(true), LastSystemVersion = state.LastSystemVersion, LightMaps = ManagedAPI.GetSharedComponentTypeHandle(), + lodCrossfadeHandle = GetComponentTypeHandle(true), #if !LATIOS_TRANSFORMS_UNCACHED_QVVS && !LATIOS_TRANSFORMS_UNITY WorldTransform = GetComponentTypeHandle(true), #elif !LATIOS_TRANSFORMS_UNCACHED_QVVS && LATIOS_TRANSFORMS_UNITY @@ -127,6 +129,7 @@ public unsafe void OnUpdate(ref SystemState state) var expandInstancesJob = new ExpandVisibleInstancesJob { DrawCommandOutput = chunkDrawCommandOutput, + crossfadesPtrMap = lodCrossfadePtrMap.chunkIdentifierToPtrMap }; var generateDrawCommandsJob = new GenerateDrawCommandsJob @@ -234,6 +237,7 @@ unsafe struct EmitDrawCommandsJob : IJobParallelForDefer [ReadOnly] public NativeArray chunksToProcess; [ReadOnly] public ComponentTypeHandle chunkPerCameraCullingMaskHandle; [ReadOnly] public ComponentTypeHandle chunkPerCameraCullingSplitsMaskHandle; + [ReadOnly] public ComponentTypeHandle lodCrossfadeHandle; public bool splitsAreValid; //[ReadOnly] public IndirectList VisibilityItems; @@ -302,6 +306,7 @@ void Execute(in ArchetypeChunk chunk) bool hasPostProcess = chunk.Has(ref PostProcessMatrix); bool isDepthSorted = chunk.Has(ref DepthSorted); bool isLightMapped = chunk.GetSharedComponentIndex(LightMaps) >= 0; + bool hasLodCrossfade = chunk.Has(ref lodCrossfadeHandle); // Check if the chunk has statically disabled motion (i.e. never in motion pass) // or enabled motion (i.e. in motion pass if there was actual motion or force-to-zero). @@ -373,6 +378,9 @@ void Execute(in ArchetypeChunk chunk) if (isLightMapped) drawCommandFlags |= BatchDrawCommandFlags.IsLightMapped; + if (hasLodCrossfade) + drawCommandFlags |= BatchDrawCommandFlags.LODCrossFade; + // Depth sorted draws are emitted with access to entity transforms, // so they can also be written out for sorting if (isDepthSorted) @@ -473,7 +481,8 @@ private void EmitDrawCommand(in DrawCommandSettings settings, int entityQword, i [BurstCompile] internal unsafe struct ExpandVisibleInstancesJob : IJobParallelForDefer { - public ChunkDrawCommandOutput DrawCommandOutput; + public ChunkDrawCommandOutput DrawCommandOutput; + [ReadOnly] public NativeHashMap crossfadesPtrMap; public void Execute(int index) { @@ -482,11 +491,12 @@ public void Execute(int index) var transformHeader = workItem.TransformArrays; int binIndex = workItem.BinIndex; - var bin = DrawCommandOutput.BinIndices.ElementAt(binIndex); - int binInstanceOffset = bin.InstanceOffset; - int binPositionOffset = bin.PositionOffset; - int workItemInstanceOffset = workItem.PrefixSumNumInstances; - int headerInstanceOffset = 0; + ref var settings = ref DrawCommandOutput.UnsortedBins.ElementAt(binIndex); + var bin = DrawCommandOutput.BinIndices.ElementAt(binIndex); + int binInstanceOffset = bin.InstanceOffset; + int binPositionOffset = bin.PositionOffset; + int workItemInstanceOffset = workItem.PrefixSumNumInstances; + int headerInstanceOffset = 0; int* visibleInstances = DrawCommandOutput.CullingOutputDrawCommands->visibleInstances; float3* sortingPositions = (float3*)DrawCommandOutput.CullingOutputDrawCommands->instanceSortingPositions; @@ -495,10 +505,11 @@ public void Execute(int index) { while (header != null) { - ExpandArray( - visibleInstances, - header, - binInstanceOffset + workItemInstanceOffset + headerInstanceOffset); + ExpandArray(visibleInstances, + header, + binInstanceOffset + workItemInstanceOffset + headerInstanceOffset, + settings.BatchID.value, + (settings.Flags & BatchDrawCommandFlags.LODCrossFade) == BatchDrawCommandFlags.LODCrossFade); headerInstanceOffset += header->NumInstances; header = header->Next; @@ -513,13 +524,14 @@ public void Execute(int index) int instanceOffset = binInstanceOffset + workItemInstanceOffset + headerInstanceOffset; int positionOffset = binPositionOffset + workItemInstanceOffset + headerInstanceOffset; - ExpandArrayWithPositions( - visibleInstances, - sortingPositions, - header, - transformHeader, - instanceOffset, - positionOffset); + ExpandArrayWithPositions(visibleInstances, + sortingPositions, + header, + transformHeader, + instanceOffset, + positionOffset, + settings.BatchID.value, + (settings.Flags & BatchDrawCommandFlags.LODCrossFade) == BatchDrawCommandFlags.LODCrossFade); headerInstanceOffset += header->NumInstances; header = header->Next; @@ -531,14 +543,26 @@ public void Execute(int index) private int ExpandArray( int* visibleInstances, DrawStream.Header* header, - int instanceOffset) + int instanceOffset, + uint batchID, + bool usesCrossfades) { int numStructs = header->NumElements; for (int i = 0; i < numStructs; ++i) { - var visibility = *header->Element(i); - int numInstances = ExpandVisibility(visibleInstances + instanceOffset, visibility); + var visibility = *header->Element(i); + int numInstances; + if (usesCrossfades) + { + var ptr = crossfadesPtrMap[new LODCrossfadePtrMap.ChunkIdentifier { batchID = batchID, batchStartIndex = visibility.ChunkStartIndex }]; + numInstances = ExpandVisibilityCrossfade( + visibleInstances + instanceOffset, + visibility, + ptr.ptr); + } + else + numInstances = ExpandVisibility(visibleInstances + instanceOffset, visibility); Assert.IsTrue(numInstances > 0); instanceOffset += numInstances; } @@ -552,19 +576,35 @@ private int ExpandArrayWithPositions( DrawStream.Header* header, DrawStream.Header* transformHeader, int instanceOffset, - int positionOffset) + int positionOffset, + uint batchID, + bool usesCrossfades) { int numStructs = header->NumElements; for (int i = 0; i < numStructs; ++i) { - var visibility = *header->Element(i); - var transforms = (TransformQvvs*)(*transformHeader->Element(i)); - int numInstances = ExpandVisibilityWithPositions( - visibleInstances + instanceOffset, - sortingPositions + positionOffset, - visibility, - transforms); + var visibility = *header->Element(i); + var transforms = (TransformQvvs*)(*transformHeader->Element(i)); + int numInstances; + if (usesCrossfades) + { + var ptr = crossfadesPtrMap[new LODCrossfadePtrMap.ChunkIdentifier { batchID = batchID, batchStartIndex = visibility.ChunkStartIndex }]; + numInstances = ExpandVisibilityWithPositionsCrossfade( + visibleInstances + instanceOffset, + sortingPositions + positionOffset, + visibility, + transforms, + ptr.ptr + ); + } + else + { + numInstances = ExpandVisibilityWithPositions(visibleInstances + instanceOffset, + sortingPositions + positionOffset, + visibility, + transforms); + } Assert.IsTrue(numInstances > 0); instanceOffset += numInstances; positionOffset += numInstances; @@ -596,6 +636,29 @@ private int ExpandVisibility(int* outputInstances, DrawCommandVisibility visibil return numInstances; } + private int ExpandVisibilityCrossfade(int* outputInstances, DrawCommandVisibility visibility, byte* crossfades) + { + int numInstances = 0; + int startIndex = visibility.ChunkStartIndex; + + for (int i = 0; i < 2; ++i) + { + ulong qword = visibility.VisibleInstances[i]; + while (qword != 0) + { + int bitIndex = math.tzcnt(qword); + ulong mask = 1ul << bitIndex; + qword ^= mask; + int instanceIndex = (i << 6) + bitIndex; + int visibilityIndex = ((startIndex + instanceIndex) & 0x00ffffff) | (crossfades[instanceIndex] << 24); + outputInstances[numInstances] = visibilityIndex; + ++numInstances; + } + } + + return numInstances; + } + private int ExpandVisibilityWithPositions( int* outputInstances, float3* outputSortingPosition, @@ -629,6 +692,41 @@ private int ExpandVisibilityWithPositions( return numInstances; } + + private int ExpandVisibilityWithPositionsCrossfade( + int* outputInstances, + float3* outputSortingPosition, + DrawCommandVisibility visibility, + TransformQvvs* transforms, + byte* crossfades) + { + int numInstances = 0; + int startIndex = visibility.ChunkStartIndex; + + for (int i = 0; i < 2; ++i) + { + ulong qword = visibility.VisibleInstances[i]; + while (qword != 0) + { + int bitIndex = math.tzcnt(qword); + ulong mask = 1ul << bitIndex; + qword ^= mask; + int instanceIndex = (i << 6) + bitIndex; + + int visibilityIndex = startIndex + instanceIndex; + outputInstances[numInstances] = (visibilityIndex & 0x00ffffff) | (crossfades[instanceIndex] << 24); +#if !LATIOS_TRANSFORMS_UNITY + outputSortingPosition[numInstances] = transforms[instanceIndex].position; +#else + outputSortingPosition[numInstances] = ((float4x4*)transforms)[instanceIndex].c3.xyz; +#endif + + ++numInstances; + } + } + + return numInstances; + } } #endif @@ -639,6 +737,7 @@ unsafe struct EmitDrawCommandsJob : IJobParallelForDefer [ReadOnly] public NativeArray chunksToProcess; [ReadOnly] public ComponentTypeHandle chunkPerCameraCullingMaskHandle; [ReadOnly] public ComponentTypeHandle chunkPerCameraCullingSplitsMaskHandle; + [ReadOnly] public ComponentTypeHandle lodCrossfadeHandle; public bool splitsAreValid; //[ReadOnly] public IndirectList VisibilityItems; @@ -707,6 +806,7 @@ void Execute(in ArchetypeChunk chunk) bool hasPostProcess = chunk.Has(ref PostProcessMatrix); bool isDepthSorted = chunk.Has(ref DepthSorted); bool isLightMapped = chunk.GetSharedComponentIndex(LightMaps) >= 0; + bool hasLodCrossfade = chunk.Has(ref lodCrossfadeHandle); // Check if the chunk has statically disabled motion (i.e. never in motion pass) // or enabled motion (i.e. in motion pass if there was actual motion or force-to-zero). @@ -778,6 +878,9 @@ void Execute(in ArchetypeChunk chunk) if (isLightMapped) drawCommandFlags |= BatchDrawCommandFlags.IsLightMapped; + if (hasLodCrossfade) + drawCommandFlags |= BatchDrawCommandFlags.LODCrossFade; + // Depth sorted draws are emitted with access to entity transforms, // so they can also be written out for sorting if (isDepthSorted) diff --git a/Kinemation/Systems/KinemationSuperSystems.cs b/Kinemation/Systems/KinemationSuperSystems.cs index cd0a844..fb16619 100644 --- a/Kinemation/Systems/KinemationSuperSystems.cs +++ b/Kinemation/Systems/KinemationSuperSystems.cs @@ -132,6 +132,7 @@ protected override void CreateSystems() EnableSystemSorting = false; GetOrCreateAndAddManagedSystem(); + GetOrCreateAndAddUnmanagedSystem(); GetOrCreateAndAddUnmanagedSystem(); GetOrCreateAndAddUnmanagedSystem(); GetOrCreateAndAddUnmanagedSystem(); diff --git a/Kinemation/Systems/LatiosEntitiesGraphicsSystem.cs b/Kinemation/Systems/LatiosEntitiesGraphicsSystem.cs index 88a5756..7196f0a 100644 --- a/Kinemation/Systems/LatiosEntitiesGraphicsSystem.cs +++ b/Kinemation/Systems/LatiosEntitiesGraphicsSystem.cs @@ -909,9 +909,11 @@ private JobHandle UpdateAllBatches(JobHandle inputDependencies, out int totalChu Profiler.EndSample(); - var numNewChunksArray = CollectionHelper.CreateNativeArray(1, WorldUpdateAllocator); - totalChunks = m_EntitiesGraphicsRenderedQuery.CalculateChunkCountWithoutFiltering(); - var newChunks = CollectionHelper.CreateNativeArray(totalChunks, WorldUpdateAllocator, NativeArrayOptions.UninitializedMemory); + var numNewChunksArray = CollectionHelper.CreateNativeArray(1, WorldUpdateAllocator); + var totalChunksFromNormalQuery = m_EntitiesGraphicsRenderedQuery.CalculateChunkCountWithoutFiltering(); + var totalChunksFromMetaQuery = m_MetaEntitiesForHybridRenderableChunksQuery.CalculateEntityCountWithoutFiltering(); + totalChunks = math.max(totalChunksFromNormalQuery, totalChunksFromMetaQuery); // If these are different for some reason, reserve the larger. + var newChunks = CollectionHelper.CreateNativeArray(totalChunks, WorldUpdateAllocator, NativeArrayOptions.UninitializedMemory); var classifyNewChunksJob = new ClassifyNewChunksJobLatiosVersion { diff --git a/Kinemation/Systems/PostBatching/PrepareLODsSystem.cs b/Kinemation/Systems/PostBatching/PrepareLODsSystem.cs new file mode 100644 index 0000000..dace9dd --- /dev/null +++ b/Kinemation/Systems/PostBatching/PrepareLODsSystem.cs @@ -0,0 +1,81 @@ +using Unity.Burst; +using Unity.Burst.Intrinsics; +using Unity.Collections; +using Unity.Entities; +using Unity.Entities.Exposed; +using Unity.Jobs; +using Unity.Mathematics; +using Unity.Rendering; + +using static Unity.Entities.SystemAPI; + +namespace Latios.Kinemation.Systems +{ + [DisableAutoCreation] + [BurstCompile] + public partial struct PrepareLODsSystem : ISystem + { + LatiosWorldUnmanaged latiosWorld; + + EntityQuery m_lodCrossfadeQuery; + + [BurstCompile] + public void OnCreate(ref SystemState state) + { + latiosWorld = state.GetLatiosWorldUnmanaged(); + + m_lodCrossfadeQuery = state.Fluent().With(true).With(true, true).Build(); + latiosWorld.worldBlackboardEntity.AddOrSetCollectionComponentAndDisposeOld(default); + } + + [BurstCompile] + public void OnDestroy(ref SystemState state) + { + } + + [BurstCompile] + public void OnUpdate(ref SystemState state) + { + // Todo? Batches not accounted for in hashmap? + var crossfadePtrMap = new NativeHashMap( + 1, + state.WorldUpdateAllocator); + latiosWorld.worldBlackboardEntity.SetCollectionComponentAndDisposeOld(new LODCrossfadePtrMap { chunkIdentifierToPtrMap = crossfadePtrMap }); + + if (!m_lodCrossfadeQuery.IsEmptyIgnoreFilter) + { + state.Dependency = new CaptureLodCrossfadePtrsJob + { + lodCrossfadeHandle = GetComponentTypeHandle(true), + entitiesGraphicsChunkInfoHandle = GetComponentTypeHandle(true), + map = crossfadePtrMap, + chunkCount = m_lodCrossfadeQuery.CalculateChunkCountWithoutFiltering() + }.Schedule(m_lodCrossfadeQuery, state.Dependency); + } + } + + // Schedule single-threaded + [BurstCompile] + struct CaptureLodCrossfadePtrsJob : IJobChunk + { + [ReadOnly] public ComponentTypeHandle lodCrossfadeHandle; + [ReadOnly] public ComponentTypeHandle entitiesGraphicsChunkInfoHandle; + + public NativeHashMap map; + public int chunkCount; + + public unsafe void Execute(in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask) + { + if (map.IsEmpty) + map.Capacity = chunkCount; + + var info = chunk.GetChunkComponentRefRO(ref entitiesGraphicsChunkInfoHandle); + var identifier = new LODCrossfadePtrMap.ChunkIdentifier { batchID = (uint)info.ValueRO.BatchIndex, batchStartIndex = info.ValueRO.CullingData.ChunkOffsetInBatch }; + var ptr = new LODCrossfadePtrMap.CrossfadePtr { ptr = (byte*)chunk.GetRequiredComponentDataPtrRO(ref lodCrossfadeHandle) }; + map.Add(identifier, ptr); + } + } + } +} + diff --git a/Kinemation/Systems/PostBatching/PrepareLODsSystem.cs.meta b/Kinemation/Systems/PostBatching/PrepareLODsSystem.cs.meta new file mode 100644 index 0000000..f135801 --- /dev/null +++ b/Kinemation/Systems/PostBatching/PrepareLODsSystem.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8293a5f7b8454dd408a9efaf62401c43 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Kinemation/Utilities/RuntimeSkeletonBlobBuilders.cs b/Kinemation/Utilities/RuntimeSkeletonBlobBuilders.cs new file mode 100644 index 0000000..7204b73 --- /dev/null +++ b/Kinemation/Utilities/RuntimeSkeletonBlobBuilders.cs @@ -0,0 +1,272 @@ +using System; +using System.Collections.Generic; +using Latios.Authoring; +using Latios.Kinemation.Authoring; +using Latios.Transforms; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Entities; +using Unity.Mathematics; +using UnityEngine; +using UnityEngine.Jobs; + +namespace Latios.Kinemation.RuntimeBlobBuilders +{ + // Main thread only - no Burst + public struct SkeletonClipSetSampler : IDisposable + { + static Queue<(Transform, int)> s_breadthQueue = new Queue<(Transform, int)>(); + static List s_transformsCache = new List(); + + internal GameObject shadowHierarchy; + internal TransformAccessArray taa; + internal NativeList parentIndices; + + public SkeletonClipSetSampler(Animator referenceAnimator) + { + shadowHierarchy = ShadowHierarchyBuilder.BuildShadowHierarchy(referenceAnimator.gameObject, !referenceAnimator.hasTransformHierarchy); + ShadowHierarchyBuilder.DeleteSkinnedMeshPaths(shadowHierarchy); + parentIndices = new NativeList(Allocator.Persistent); // TAA doesn't have an allocator, so we use persistent to match its behavior. + + s_breadthQueue.Clear(); + s_transformsCache.Clear(); + + s_breadthQueue.Enqueue((shadowHierarchy.transform, -1)); + + while (s_breadthQueue.Count > 0) + { + var (bone, parentIndex) = s_breadthQueue.Dequeue(); + int currentIndex = parentIndices.Length; + parentIndices.Add((short)parentIndex); + s_transformsCache.Add(bone); + + for (int i = 0; i < bone.childCount; i++) + { + var child = bone.GetChild(i); + s_breadthQueue.Enqueue((child, currentIndex)); + } + } + + taa = new TransformAccessArray(s_transformsCache.Count); + foreach (var tf in s_transformsCache) + taa.Add(tf); + shadowHierarchy.SetActive(false); + } + + // -1 for no parent + public short GetBoneParent(int boneIndex) => parentIndices[boneIndex]; + + public string GetNameOfBone(int boneIndex) => taa[boneIndex].gameObject.name; + + public unsafe SkeletonClipSetSampleData Sample(ReadOnlySpan clips, AllocatorManager.AllocatorHandle allocator) + { + shadowHierarchy.SetActive(true); + SkeletonClipSetSampleData result = default; + result.clips = new UnsafeList(clips.Length, allocator); + result.clips.Length = clips.Length; + + int totalRequiredTransforms = 0; + int totalRequiredEvents = 0; + for (int i = 0; i < clips.Length; i++) + { + var clipConfig = clips[i]; + var clip = clipConfig.clip.Value; + int requiredSamples = Mathf.CeilToInt(clip.frameRate * clip.length) + (clipConfig.settings.copyFirstKeyAtEnd ? 1 : 0); + var requiredTransforms = requiredSamples * taa.length; + + ref var output = ref result.clips.ElementAt(i); + output = new SkeletonClipToBake + { + clip = clipConfig.clip, + settings = clipConfig.settings, + eventsStart = totalRequiredEvents, + eventsCount = clipConfig.events.Length, + clipName = clip.name, + sampleRate = clip.frameRate, + boneTransformStart = totalRequiredTransforms, + boneTransformCount = requiredTransforms + }; + totalRequiredTransforms += requiredTransforms; + totalRequiredEvents += clipConfig.events.Length; + } + result.boneSamplesBuffer = new UnsafeList(totalRequiredTransforms, allocator); + result.boneSamplesBuffer.Length = totalRequiredTransforms; + result.events = new UnsafeList(totalRequiredEvents, allocator); + + for (int i = 0; i < clips.Length; i++) + { + var clipConfig = clips[i]; + var clip = result.clips[i]; + + SampleClip(ref result.boneSamplesBuffer, + clipConfig.clip.Value, + clip.boneTransformStart, + clipConfig.settings.copyFirstKeyAtEnd, + clipConfig.settings.rootMotionOverrideMode); + if (clipConfig.events.IsCreated && clipConfig.events.Length > 0) + result.events.AddRangeNoResize(clipConfig.events.GetUnsafeReadOnlyPtr(), clipConfig.events.Length); + } + result.parentIndices = new UnsafeList(parentIndices.Length, allocator); + result.parentIndices.AddRangeNoResize(*parentIndices.GetUnsafeList()); + + shadowHierarchy.SetActive(false); + + return result; + } + + public void Dispose() + { + taa.Dispose(); + shadowHierarchy.DestroySafelyFromAnywhere(); + parentIndices.Dispose(); + } + + void SampleClip(ref UnsafeList boneTransforms, + AnimationClip clip, + int startIndex, + bool copyFirstPose, + SkeletonClipCompressionSettings.RootMotionOverrideMode rootMotionMode) + { + int requiredSamples = Mathf.CeilToInt(clip.frameRate * clip.length) + (copyFirstPose ? 1 : 0); + + var oldWrapMode = clip.wrapMode; + clip.wrapMode = WrapMode.Clamp; + Animator animator = null; + bool backupRootMotionSettings = false; + if (rootMotionMode != SkeletonClipCompressionSettings.RootMotionOverrideMode.UseAnimatorSettings) + { + animator = shadowHierarchy.GetComponent(); + backupRootMotionSettings = animator.applyRootMotion; + animator.applyRootMotion = rootMotionMode == SkeletonClipCompressionSettings.RootMotionOverrideMode.EnableRootMotion; + } + + float timestep = math.rcp(clip.frameRate); + var job = new CaptureBoneSamplesJob + { + boneTransforms = boneTransforms, + samplesPerBone = requiredSamples, + currentSample = 0, + startOffset = startIndex, + }; + + if (copyFirstPose) + requiredSamples--; + + for (int i = 0; i < requiredSamples; i++) + { + clip.SampleAnimation(shadowHierarchy, timestep * i); + job.currentSample = i; + job.RunReadOnly(taa); + } + + if (copyFirstPose) + { + clip.SampleAnimation(shadowHierarchy, 0f); + job.currentSample = requiredSamples; + job.RunReadOnly(taa); + } + + if (animator != null) + animator.applyRootMotion = backupRootMotionSettings; + clip.wrapMode = oldWrapMode; + } + + [BurstCompile] + struct CaptureBoneSamplesJob : IJobParallelForTransform + { + public UnsafeList boneTransforms; + public int samplesPerBone; + public int currentSample; + public int startOffset; + + public void Execute(int index, TransformAccess transform) + { + int target = startOffset + index * samplesPerBone + currentSample; + boneTransforms[target] = new TransformQvvs(transform.localPosition, transform.localRotation, 1f, transform.localScale); + } + } + } + + // Job and Burst compatible + public struct SkeletonClipSetSampleData : IDisposable + { + internal UnsafeList boneSamplesBuffer; + internal UnsafeList parentIndices; + internal UnsafeList clips; + internal UnsafeList events; + + public int boneCount => parentIndices.Length; + + // -1 for no parent + public short GetBoneParent(int boneIndex) => parentIndices[boneIndex]; + + public TransformQvvs SampleBone(int clipIndex, int boneIndex, int sampleIndex) + { + var clip = clips[clipIndex]; + var target = clip.boneTransformStart + boneIndex * clip.boneTransformCount + sampleIndex; + return boneSamplesBuffer[target]; + } + + public unsafe void BuildBlob(ref BlobBuilder builder, ref SkeletonClipSetBlob blob) + { + var boneTransforms = CollectionHelper.ConvertExistingDataToNativeArray(boneSamplesBuffer.Ptr, boneSamplesBuffer.Length, Allocator.None, true); + var eventsArray = CollectionHelper.ConvertExistingDataToNativeArray(events.Ptr, events.Length, Allocator.None, true); + + blob.boneCount = (short)parentIndices.Length; + var blobClips = builder.Allocate(ref blob.clips, clips.Length); + + // Step 1: Patch parent hierarchy for ACL + var parents = new NativeArray(parentIndices.Length, Allocator.Temp); + for (short i = 0; i < parentIndices.Length; i++) + { + short index = parentIndices[i]; + if (index < 0) + index = i; + parents[i] = index; + } + + int clipIndex = 0; + foreach (var srcClip in clips) + { + // Step 2: Convert settings + var aclSettings = new AclUnity.Compression.SkeletonCompressionSettings + { + compressionLevel = srcClip.settings.compressionLevel, + maxDistanceError = srcClip.settings.maxDistanceError, + maxUniformScaleError = srcClip.settings.maxUniformScaleError, + sampledErrorDistanceFromBone = srcClip.settings.sampledErrorDistanceFromBone + }; + + // Step 3: Encode bone samples into QVV array + var qvvArray = boneTransforms.GetSubArray(srcClip.boneTransformStart, srcClip.boneTransformCount); + + // Step 4: Compress + var compressedClip = AclUnity.Compression.CompressSkeletonClip(parents, qvvArray, srcClip.sampleRate, aclSettings); + + // Step 5: Build blob clip + blobClips[clipIndex] = default; + blobClips[clipIndex].name = srcClip.clipName; + var eventsRange = eventsArray.GetSubArray(srcClip.eventsStart, srcClip.eventsCount); + ClipEventsBlobHelpers.Convert(ref blobClips[clipIndex].events, ref builder, eventsRange); + + var compressedData = builder.Allocate(ref blobClips[clipIndex].compressedClipDataAligned16, compressedClip.sizeInBytes, 16); + compressedClip.CopyTo((byte*)compressedData.GetUnsafePtr()); + + // Step 6: Dispose ACL memory and safety + compressedClip.Dispose(); + + clipIndex++; + } + } + + public void Dispose() + { + boneSamplesBuffer.Dispose(); + parentIndices.Dispose(); + clips.Dispose(); + events.Dispose(); + } + } +} + diff --git a/Kinemation/Utilities/RuntimeSkeletonBlobBuilders.cs.meta b/Kinemation/Utilities/RuntimeSkeletonBlobBuilders.cs.meta new file mode 100644 index 0000000..448044d --- /dev/null +++ b/Kinemation/Utilities/RuntimeSkeletonBlobBuilders.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 29afbbcf7ad29d742b719d851373046c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/MyriAudio/Components/EffectsComponents.cs b/MyriAudio/Components/EffectsComponents.cs index 5bcefcb..a2707cb 100644 --- a/MyriAudio/Components/EffectsComponents.cs +++ b/MyriAudio/Components/EffectsComponents.cs @@ -18,8 +18,30 @@ internal interface ISpatialEffectParamaters : IComponentDa { } - internal interface IListenerProperty : IComponentData + internal interface IResourceComponent : IComponentData { } + + internal interface IResourceBuffer : IBufferElementData + { + } + + internal interface IFeedbackComponent : IComponentData, IEnableableComponent + { + } + + internal interface IFeedbackBuffer : IBufferElementData, IEnableableComponent + { + } + + /// + /// Provides more granular control of sending to the DSP thread than change filters + /// + internal struct DspSubmitFlag : IComponentData, IEnableableComponent { } + + internal struct EffectStackElement : IBufferElementData, IEnableableComponent + { + public Entity effectEntity; + } } diff --git a/MyriAudio/DSP/BuiltInEffects/VirtualOutputEffect.cs b/MyriAudio/DSP/BuiltInEffects/VirtualOutputEffect.cs index d0b1cb1..d24acfd 100644 --- a/MyriAudio/DSP/BuiltInEffects/VirtualOutputEffect.cs +++ b/MyriAudio/DSP/BuiltInEffects/VirtualOutputEffect.cs @@ -4,6 +4,7 @@ namespace Latios.Myri.DSP { + [EffectOptions(EffectOptionFlags.RequireUpdateWhenCulled | EffectOptionFlags.RequireUpdateWhenInputFrameDisconnected)] internal struct VirtualOutputEffect : IEffect { internal unsafe struct Ptr @@ -28,20 +29,22 @@ public bool TryGetFrame(Entity stackEntity, int indexInStack, int currentFrame, if (m_isInListenerStack) { - if (m_listenerPreviousStackFrame.frameIndex == currentFrame) + if (m_currentStackFrame.frameIndex == currentFrame) + { frame = m_listenerPreviousStackFrame.readOnly; - else + return true; + } + else if (m_currentStackFrame.frameIndex == currentFrame - 1) + { frame = m_currentStackFrame.readOnly; - return true; + return true; + } } frame = default; return false; } - public bool RequireUpdateWhenCulled => true; - public bool RequireUpdateWhenInputFrameDisconnected => true; - public void OnAwake(in EffectContext context, in VirtualOutputParameters parameters) { } diff --git a/MyriAudio/DSP/DSPContexts.cs b/MyriAudio/DSP/DSPContexts.cs index a025453..befa6e5 100644 --- a/MyriAudio/DSP/DSPContexts.cs +++ b/MyriAudio/DSP/DSPContexts.cs @@ -29,15 +29,18 @@ internal enum StackType : byte internal unsafe struct UpdateContext { - public Entity stackEntity; - public TransformQvvs stackTransform => *stackTransformPtr; - public int indexInStack; - public uint layerMask; + public Entity stackEntity => stackType == StackType.Source ? sourcePtr->sourceEntity : listenerPtr->listenerEntity; + public TransformQvvs stackTransform => stackType == StackType.Source ? sourcePtr->worldTransform : listenerPtr->worldTransform; + public int indexInStack; + public uint layerMask => stackType == StackType.Source ? (1u << sourcePtr->layerIndex) : listenerPtr->layerMask; public bool isCulled; public StackType stackType; - internal TransformQvvs* stackTransformPtr; + internal void* metadataPtr; + internal Interop.SourceStackMetadata* sourcePtr => (Interop.SourceStackMetadata*)metadataPtr; + internal Interop.ListenerStackMetadata* listenerPtr => (Interop.ListenerStackMetadata*)metadataPtr; internal NativeHashMap virtualOutputsMap; + internal NativeHashMap resourcesMap; internal int currentFrame; public bool TryGetVirtualOutput(Entity virtualOutputEffectEntity, out SampleFrame.ReadOnly virtualFrame) @@ -49,12 +52,41 @@ public bool TryGetVirtualOutput(Entity virtualOutputEffectEntity, out SampleFram virtualFrame = default; return false; } + + public bool TryGetResourceComponent(Entity resourceEntity, out DSPRef resource) where T : unmanaged, IResourceComponent + { + var key = new ResourceKey { componentType = ComponentType.ReadWrite(), entity = resourceEntity }; + var result = resourcesMap.TryGetValue(key, out var candidate); + var metadataPtr = (Interop.ResourceComponentMetadata*)candidate.metadataPtr; + if (metadataPtr->enabled) + { + resource = new DSPRef((T*)metadataPtr->componentPtr); + return true; + } + resource = default; + return false; + } + + public bool TryGetResourceBuffer(Entity resourceEntity, out ReadOnlySpan resource) where T : unmanaged, IResourceBuffer + { + var key = new ResourceKey { componentType = ComponentType.ReadWrite(), entity = resourceEntity }; + var result = resourcesMap.TryGetValue(key, out var candidate); + var metadataPtr = (Interop.ResourceBufferMetadata*)candidate.metadataPtr; + if (metadataPtr->enabled) + { + resource = new ReadOnlySpan(metadataPtr->bufferPtr, metadataPtr->elementCount); + return true; + } + resource = default; + return false; + } } internal unsafe struct SpatialCullingContext { internal UnsafeList listeners; internal Interop.SourceStackMetadata* sourcePtr; + internal NativeHashMap resourcesMap; public int listenerCount => listeners.Length; public Entity sourceEntity => sourcePtr->sourceEntity; @@ -64,20 +96,32 @@ internal unsafe struct SpatialCullingContext public Entity GetListenerEntity(int index) => listeners[index].ptr->listenerEntity; public TransformQvvs GetListenerTransform(int index) => listeners[index].ptr->worldTransform; public uint GetListenerLayerMask(int index) => listeners[index].ptr->layerMask; - public bool TryGetListenerProperty(int index, out DSPRef property) where T : unmanaged, IListenerProperty + + public bool TryGetResourceComponent(Entity resourceEntity, out DSPRef resource) where T : unmanaged, IResourceComponent { - var ptr = listeners[index]; - var requestedType = ComponentType.ReadWrite(); - for (int i = 0; i < ptr.ptr->listenerPropertiesCount; i++) + var key = new ResourceKey { componentType = ComponentType.ReadWrite(), entity = resourceEntity }; + var result = resourcesMap.TryGetValue(key, out var candidate); + var metadataPtr = (Interop.ResourceComponentMetadata*)candidate.metadataPtr; + if (metadataPtr->enabled) { - if (ptr.ptr->listenerProperties[i].propertyType.TypeIndex == requestedType.TypeIndex) - { - property = new DSPRef((T*)ptr.ptr->listenerProperties[i].propertyPtr); - return true; - } + resource = new DSPRef((T*)metadataPtr->componentPtr); + return true; } + resource = default; + return false; + } - property = default; + public bool TryGetResourceBuffer(Entity resourceEntity, out ReadOnlySpan resource) where T : unmanaged, IResourceBuffer + { + var key = new ResourceKey { componentType = ComponentType.ReadWrite(), entity = resourceEntity }; + var result = resourcesMap.TryGetValue(key, out var candidate); + var metadataPtr = (Interop.ResourceBufferMetadata*)candidate.metadataPtr; + if (metadataPtr->enabled) + { + resource = new ReadOnlySpan(metadataPtr->bufferPtr, metadataPtr->elementCount); + return true; + } + resource = default; return false; } } @@ -107,21 +151,34 @@ internal unsafe struct SpatialUpdateContext internal Interop.SourceStackMetadata* sourcePtr; internal Interop.ListenerStackMetadata* listenerPtr; internal NativeHashMap virtualOutputsMap; + internal NativeHashMap resourcesMap; internal int currentFrame; - public bool TryGetListenerProperty(out DSPRef property) where T : unmanaged, IListenerProperty + public bool TryGetResourceComponent(Entity resourceEntity, out DSPRef resource) where T : unmanaged, IResourceComponent { - var requestedType = ComponentType.ReadWrite(); - for (int i = 0; i < listenerPtr->listenerPropertiesCount; i++) + var key = new ResourceKey { componentType = ComponentType.ReadWrite(), entity = resourceEntity }; + var result = resourcesMap.TryGetValue(key, out var candidate); + var metadataPtr = (Interop.ResourceComponentMetadata*)candidate.metadataPtr; + if (metadataPtr->enabled) { - if (listenerPtr->listenerProperties[i].propertyType.TypeIndex == requestedType.TypeIndex) - { - property = new DSPRef((T*)listenerPtr->listenerProperties[i].propertyPtr); - return true; - } + resource = new DSPRef((T*)metadataPtr->componentPtr); + return true; } + resource = default; + return false; + } - property = default; + public bool TryGetResourceBuffer(Entity resourceEntity, out ReadOnlySpan resource) where T : unmanaged, IResourceBuffer + { + var key = new ResourceKey { componentType = ComponentType.ReadWrite(), entity = resourceEntity }; + var result = resourcesMap.TryGetValue(key, out var candidate); + var metadataPtr = (Interop.ResourceBufferMetadata*)candidate.metadataPtr; + if (metadataPtr->enabled) + { + resource = new ReadOnlySpan(metadataPtr->bufferPtr, metadataPtr->elementCount); + return true; + } + resource = default; return false; } diff --git a/MyriAudio/DSP/DSPInterfaces.cs b/MyriAudio/DSP/DSPInterfaces.cs index 5db0cba..2d2cd6c 100644 --- a/MyriAudio/DSP/DSPInterfaces.cs +++ b/MyriAudio/DSP/DSPInterfaces.cs @@ -5,6 +5,24 @@ namespace Latios.Myri.DSP { // Make these public on release + internal enum EffectOptionFlags + { + None = 0x0, + RequireUpdateWhenCulled = 0x1, + RequireUpdateWhenInputFrameDisconnected = 0x2, + AllowSharedInstanceBetweenStacks = 0x4, + } + + internal class EffectOptionsAttribute : System.Attribute + { + public EffectOptionFlags flags; + + public EffectOptionsAttribute(EffectOptionFlags flags) + { + this.flags = flags; + } + } + internal interface IEffect where TEffect : unmanaged, IEffect where TParameters : unmanaged, IEffectParameters @@ -13,8 +31,8 @@ internal interface IEffect public void OnUpdate(in EffectContext effectContext, in UpdateContext updateContext, in TParameters parameters, ref SampleFrame frame); public void OnDestroy(in EffectContext context); - public bool RequireUpdateWhenCulled => false; - public bool RequireUpdateWhenInputFrameDisconnected => false; + //public bool RequireUpdateWhenCulled => false; + //public bool RequireUpdateWhenInputFrameDisconnected => false; } internal interface ISpatialEffect @@ -26,8 +44,8 @@ internal interface ISpatialEffect public void OnUpdate(in EffectContext effectContext, in SpatialUpdateContext updateContext, in TParameters parameters, ref SampleFrame frame); public void OnDestroy(in EffectContext context); - public bool RequireUpdateWhenCulled => false; - public bool RequireUpdateWhenInputFrameDisconnected => false; + //public bool RequireUpdateWhenCulled => false; + //public bool RequireUpdateWhenInputFrameDisconnected => false; } } diff --git a/MyriAudio/DSP/Primitives/BrickwallLimiter.cs b/MyriAudio/DSP/Primitives/BrickwallLimiter.cs index 9272292..28c8765 100644 --- a/MyriAudio/DSP/Primitives/BrickwallLimiter.cs +++ b/MyriAudio/DSP/Primitives/BrickwallLimiter.cs @@ -160,6 +160,13 @@ public void SetLookaheadSampleCount(int lookaheadSampleCount) } public void ResetAttenuation() => m_currentAttenuationDB = 0f; + + public void ClearLookahead() + { + m_delayQueueL.Clear(); + m_delayQueueR.Clear(); + m_delayAmplitudeDB.Clear(); + } } } diff --git a/MyriAudio/DSP/Primitives/SampleFrame.cs b/MyriAudio/DSP/Primitives/SampleFrame.cs index 4e5e116..cc96d42 100644 --- a/MyriAudio/DSP/Primitives/SampleFrame.cs +++ b/MyriAudio/DSP/Primitives/SampleFrame.cs @@ -28,6 +28,12 @@ public struct ReadOnly public int frameIndex { get; internal set; } public bool connected { get; internal set; } } + + public void ClearToZero() + { + left.AsSpan().Clear(); + right.AsSpan().Clear(); + } } // General reminder: Adding dB is like multiplying raw, and subtracting dB is like dividing. diff --git a/MyriAudio/DSP/Primitives/SampleQueue.cs b/MyriAudio/DSP/Primitives/SampleQueue.cs index 181b3e5..7c7ae79 100644 --- a/MyriAudio/DSP/Primitives/SampleQueue.cs +++ b/MyriAudio/DSP/Primitives/SampleQueue.cs @@ -60,6 +60,13 @@ public float this[int index] } } + public void Clear() + { + m_nextEnqueueIndex = 0; + m_nextDequeueIndex = 0; + m_count = 0; + } + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] void CheckEnqueue() { diff --git a/MyriAudio/Internal/DspGraph/MyriMegaKernel.cs b/MyriAudio/Internal/DspGraph/MyriMegaKernel.cs index 018a334..d7239dd 100644 --- a/MyriAudio/Internal/DspGraph/MyriMegaKernel.cs +++ b/MyriAudio/Internal/DspGraph/MyriMegaKernel.cs @@ -28,13 +28,18 @@ internal unsafe partial struct MyriMegaKernel : IAudioKernel m_queuedDspUpdateBuffers; - ChunkedList128 m_effectIdToPtrMap; - ChunkedList128 m_spatialEffectIdToPtrMap; - ChunkedList128 m_sourceStackIdToPtrMap; - ChunkedList128 m_listenerStackIdToStateMap; - + ChunkedList128 m_effectIdToPtrMap; + ChunkedList128 m_spatialEffectIdToPtrMap; + ChunkedList128 m_sourceStackIdToPtrMap; + ChunkedList128 m_listenerStackIdToStateMap; + ChunkedList128 m_resourceComponentIdToPtrMap; + ChunkedList128 m_resourceBufferIdToPtrMap; + + NativeHashMap m_resourceKeyToPtrMap; NativeHashMap m_entityToVirtualOutputMap; + BrickwallLimiter m_masterLimiter; + SampleFramePool m_framePool; internal UnsafeList > m_listenersByLayer; @@ -46,32 +51,49 @@ internal unsafe partial struct MyriMegaKernel : IAudioKernel m_rewindableAllocator; ProfilerMarker m_profilingReceivedBuffers; + ProfilerMarker m_profilingSourceStacks; + ProfilerMarker m_profilingSpatializers; + ProfilerMarker m_profilingListenerStacks; + ProfilerMarker m_profilingMixdown; public void Initialize() { // We start on frame 1 so that a buffer ID and frame of both 0 means uninitialized. // The audio components use this at the time of writing this comment. - m_currentFrame = 1; - m_nextUpdateFrame = 0; - m_lastProcessedBufferID = -1; - m_listenersDirty = false; - m_hasFirstDspBuffer = false; - m_hasValidDspBuffer = false; - m_queuedDspUpdateBuffers = new UnsafeList(16, Allocator.AudioKernel); - m_effectIdToPtrMap = new ChunkedList128(Allocator.AudioKernel); - m_spatialEffectIdToPtrMap = new ChunkedList128(Allocator.AudioKernel); - m_sourceStackIdToPtrMap = new ChunkedList128(Allocator.AudioKernel); - m_listenerStackIdToStateMap = new ChunkedList128(Allocator.AudioKernel); - m_entityToVirtualOutputMap = new NativeHashMap(128, Allocator.AudioKernel); - m_framePool = new SampleFramePool(Allocator.AudioKernel); - m_listenersByLayer = new UnsafeList >(32, Allocator.AudioKernel); + m_currentFrame = 1; + m_nextUpdateFrame = 0; + m_lastProcessedBufferID = -1; + m_listenersDirty = false; + m_hasFirstDspBuffer = false; + m_hasValidDspBuffer = false; + m_queuedDspUpdateBuffers = new UnsafeList(16, Allocator.AudioKernel); + m_effectIdToPtrMap = new ChunkedList128(Allocator.AudioKernel); + m_spatialEffectIdToPtrMap = new ChunkedList128(Allocator.AudioKernel); + m_sourceStackIdToPtrMap = new ChunkedList128(Allocator.AudioKernel); + m_listenerStackIdToStateMap = new ChunkedList128(Allocator.AudioKernel); + m_resourceComponentIdToPtrMap = new ChunkedList128(Allocator.AudioKernel); + m_resourceBufferIdToPtrMap = new ChunkedList128(Allocator.AudioKernel); + m_resourceKeyToPtrMap = new NativeHashMap(256, Allocator.AudioKernel); + m_entityToVirtualOutputMap = new NativeHashMap(64, Allocator.AudioKernel); + m_framePool = new SampleFramePool(Allocator.AudioKernel); + m_listenersByLayer = new UnsafeList >(32, Allocator.AudioKernel); for (int i = 0; i < 32; i++) m_listenersByLayer.Add(new UnsafeList(8, Allocator.AudioKernel)); m_samplingCache = new SamplingCache(Allocator.AudioKernel); //m_rewindableAllocator = new AllocatorHelper(Allocator.AudioKernel); //m_rewindableAllocator.Allocator.Initialize(64 * 1024); + m_masterLimiter = new BrickwallLimiter(BrickwallLimiter.kDefaultPreGain, + BrickwallLimiter.kDefaultLimitDB, + BrickwallLimiter.kDefaultReleaseDBPerSample, + BrickwallLimiter.kDefaultLookaheadSampleCount, + Allocator.AudioKernel); + m_profilingReceivedBuffers = new ProfilerMarker("ProcessReceivedBuffers"); + m_profilingSourceStacks = new ProfilerMarker("SourceStacks"); + m_profilingSpatializers = new ProfilerMarker("Spatialization"); + m_profilingListenerStacks = new ProfilerMarker("ListenerStacks"); + m_profilingMixdown = new ProfilerMarker("Mixdown"); } public void Execute(ref ExecuteContext context) @@ -80,6 +102,9 @@ public void Execute(ref ExecuteContext context) m_sampleRate = context.SampleRate; ProcessReceivedBuffers(); + SampleSources(); + ProcessPresampledChannels(); + SampleListeners(); } public void Dispose() @@ -108,6 +133,7 @@ public void Dispose() m_framePool.Dispose(); m_listenersByLayer.Dispose(); m_samplingCache.Dispose(); + m_masterLimiter.Dispose(); //m_rewindableAllocator.Dispose(); } @@ -135,10 +161,32 @@ public void Dispose(ref SampleFramePool sampleFramePool) if (limiter.isCreated) limiter.Dispose(); listenerMetadataPtr = default; + sampleFrame = default; } } } + internal struct ResourceKey : IEquatable + { + public Entity entity; + public ComponentType componentType; + + public bool Equals(ResourceKey other) + { + return entity.Equals(other.entity) && componentType.Equals(other.componentType); + } + + public override int GetHashCode() + { + return new int2(entity.GetHashCode(), componentType.GetHashCode()).GetHashCode(); + } + } + + internal unsafe struct ResourceValue + { + public void* metadataPtr; // Determine which from key's componentType + } + internal unsafe struct ChunkedList128 : IDisposable where T : unmanaged { UnsafeList m_chunks; diff --git a/MyriAudio/Internal/DspGraph/MyriMegaKernelMixdown.cs b/MyriAudio/Internal/DspGraph/MyriMegaKernelMixdown.cs index 904426f..d2b7128 100644 --- a/MyriAudio/Internal/DspGraph/MyriMegaKernelMixdown.cs +++ b/MyriAudio/Internal/DspGraph/MyriMegaKernelMixdown.cs @@ -1,8 +1,73 @@ +using Unity.Audio; using Unity.Collections; using Unity.Entities; using Unity.Mathematics; -namespace Latios.Myri +namespace Latios.Myri.DSP { - -} \ No newline at end of file + internal unsafe partial struct MyriMegaKernel + { + public void Mixdown(ref ExecuteContext context) + { + m_profilingMixdown.Begin(); + + var finalFrame = m_framePool.Acquire(m_frameSize); + finalFrame.connected = true; + finalFrame.frameIndex = m_currentFrame; + finalFrame.ClearToZero(); + for (int listenerIndex = 0; listenerIndex < m_listenerStackIdToStateMap.length; listenerIndex++) + { + ref var listener = ref m_listenerStackIdToStateMap[listenerIndex]; + if (listener.listenerMetadataPtr == null) + continue; + + ref var listenerMeta = ref *listener.listenerMetadataPtr; + if (!listenerMeta.listenerEnabled || listenerMeta.limiterSettings.preGain == 0f) + { + if (listener.limiter.isCreated) + { + listener.limiter.ResetAttenuation(); + listener.limiter.ClearLookahead(); + } + continue; + } + + if (!listener.limiter.isCreated) + { + if (!listener.sampleFrame.connected) + continue; + + listener.limiter = new BrickwallLimiter(listenerMeta.limiterSettings.preGain, + listenerMeta.limiterSettings.limitDB, + listenerMeta.limiterSettings.releaseDBPerSample, + listenerMeta.limiterSettings.lookaheadSampleCount, + Allocator.AudioKernel); + } + else + { + listener.limiter.preGain = listenerMeta.limiterSettings.preGain; + listener.limiter.limitDB = listenerMeta.limiterSettings.limitDB; + listener.limiter.releasePerSampleDB = listenerMeta.limiterSettings.releaseDBPerSample; + listener.limiter.SetLookaheadSampleCount(listenerMeta.limiterSettings.lookaheadSampleCount); + } + + listener.limiter.ProcessFrame(ref listener.sampleFrame, true); + for (int i = 0; i < finalFrame.length; i++) + { + var left = finalFrame.left; + var right = finalFrame.right; + left[i] += listener.sampleFrame.left[i]; + right[i] += listener.sampleFrame.right[i]; + } + } + + m_masterLimiter.ProcessFrame(ref finalFrame, true); + var buffer = context.Outputs.GetSampleBuffer(0); + buffer.GetBuffer(0).CopyFrom(finalFrame.left); + buffer.GetBuffer(1).CopyFrom(finalFrame.right); + + m_profilingMixdown.End(); + } + } +} + diff --git a/MyriAudio/Internal/DspGraph/MyriMegaKernelProcessReceivedBuffers.cs b/MyriAudio/Internal/DspGraph/MyriMegaKernelProcessReceivedBuffers.cs index e56db31..08c3ff0 100644 --- a/MyriAudio/Internal/DspGraph/MyriMegaKernelProcessReceivedBuffers.cs +++ b/MyriAudio/Internal/DspGraph/MyriMegaKernelProcessReceivedBuffers.cs @@ -71,20 +71,29 @@ void ProcessReceivedBuffers() ProcessDestroyedSpatialEffects(ref updateBuffer); ProcessDestroyedSourceStacks(ref updateBuffer); ProcessDestroyedListenerStacks(ref updateBuffer); + ProcessDestroyedResourceComponents(ref updateBuffer); + ProcessDestroyedResourceBuffers(ref updateBuffer); ProcessCreatedEffects(ref updateBuffer); ProcessCreatedSpatialEffects(ref updateBuffer); ProcessCreatedSourceStacks(ref updateBuffer); ProcessCreatedListenerStacks(ref updateBuffer); + ProcessCreatedResourceComponents(ref updateBuffer); + ProcessCreatedResourceBuffers(ref updateBuffer); ProcessUpdatedEffects(ref updateBuffer); ProcessUpdatedSpatialEffects(ref updateBuffer); - ProcessFullyUpdatedSourceStacks(ref updateBuffer); + ProcessUpdatedSourceStacks(ref updateBuffer); ProcessTransformUpdatedSourceStacks(ref updateBuffer); - ProcessFullyUpdatedListenerStacks(ref updateBuffer); + ProcessUpdatedListenerStacks(ref updateBuffer); ProcessTransformUpdatedListenerStacks(ref updateBuffer); + ProcessUpdatedResourceComponents(ref updateBuffer); + ProcessUpdatedResourceBuffers(ref updateBuffer); + + ProcessUpdatedEnabledStates(ref updateBuffer); } m_dspUpdateBuffer = m_queuedDspUpdateBuffers[bestIndex]; + ProcessMaster(ref m_dspUpdateBuffer); m_queuedDspUpdateBuffers.RemoveRange(0, bestIndex + 1); m_hasFirstDspBuffer = true; m_hasValidDspBuffer = true; @@ -134,11 +143,8 @@ void ProcessCreatedEffects(ref DspUpdateBuffer updateBuffer) persistentAllocator = Allocator.AudioKernel, //tempAllocator = m_rewindableAllocator.Allocator.Handle }, - effectPtr = null, - parametersPtr = null, - requiresUpdateWhenCulled = false, - requiresUpdateWhenInputFrameDisconnected = false, - isVirtualOutput = false, + effectPtr = null, + parametersPtr = null, }; foreach (var newEffectPtr in updateBuffer.effectsUpdateBuffer.newEffects) @@ -147,12 +153,9 @@ void ProcessCreatedEffects(ref DspUpdateBuffer updateBuffer) op.parametersPtr = newEffectPtr.ptr->parametersPtr; op.effectPtr = newEffectPtr.ptr->effectPtr; EffectOperations.InitEffect(ref op, newEffectPtr.ptr->functionPtr); - newEffectPtr.ptr->requiresUpdateWhenCulled = op.requiresUpdateWhenCulled; - newEffectPtr.ptr->requiresUpdateWhenInputFrameDisconnected = op.requiresUpdateWhenInputFrameDisconnected; - newEffectPtr.ptr->isVirtualOutput = op.isVirtualOutput; - m_effectIdToPtrMap[newEffectPtr.ptr->effectId] = newEffectPtr; + m_effectIdToPtrMap[newEffectPtr.ptr->effectId] = newEffectPtr; - if (op.isVirtualOutput) + if (newEffectPtr.ptr->isVirtualOutput) { m_entityToVirtualOutputMap.Add(op.effectContext.effectEntity, new VirtualOutputEffect.Ptr { @@ -229,9 +232,7 @@ void ProcessCreatedSpatialEffects(ref DspUpdateBuffer updateBuffer) op.parametersPtr = newEffectPtr.ptr->parametersPtr; op.effectPtr = newEffectPtr.ptr->effectPtr; EffectOperations.InitSpatialEffect(ref op, newEffectPtr.ptr->functionPtr); - newEffectPtr.ptr->requiresUpdateWhenCulled = op.requiresUpdateWhenCulled; - newEffectPtr.ptr->requiresUpdateWhenInputFrameDisconnected = op.requiresUpdateWhenInputFrameDisconnected; - m_spatialEffectIdToPtrMap[newEffectPtr.ptr->effectId] = newEffectPtr; + m_spatialEffectIdToPtrMap[newEffectPtr.ptr->effectId] = newEffectPtr; } } @@ -278,23 +279,6 @@ void ProcessCreatedSourceStacks(ref DspUpdateBuffer updateBuffer) { foreach (var newStack in updateBuffer.sourceStacksUpdateBuffer.newSourceStacks) { - for (int i = 0; i < newStack.ptr->effectIDsCount; i++) - { - ref var element = ref newStack.ptr->effectIDs[i]; - - if (element.isSpatialEffect) - { - var ptr = m_spatialEffectIdToPtrMap[element.effectId].ptr; - element.requiresUpdateWhenCulled = ptr->requiresUpdateWhenCulled; - element.requiresUpdateWhenInputFrameDisconnected = ptr->requiresUpdateWhenInputFrameDisconnected; - } - else - { - var ptr = m_effectIdToPtrMap[element.effectId].ptr; - element.requiresUpdateWhenCulled = ptr->requiresUpdateWhenCulled; - element.requiresUpdateWhenInputFrameDisconnected = ptr->requiresUpdateWhenInputFrameDisconnected; - } - } m_sourceStackIdToPtrMap[newStack.ptr->sourceId] = newStack; } } @@ -307,33 +291,14 @@ void ProcessDestroyedSourceStacks(ref DspUpdateBuffer updateBuffer) } } - void ProcessFullyUpdatedSourceStacks(ref DspUpdateBuffer updateBuffer) + void ProcessUpdatedSourceStacks(ref DspUpdateBuffer updateBuffer) { foreach (var newStack in updateBuffer.sourceStacksUpdateBuffer.updatedSourceStacks) { - ref var storedStackPtr = ref m_sourceStackIdToPtrMap[newStack.ptr->sourceId]; - var oldPtr = storedStackPtr.ptr; - storedStackPtr = newStack; - if (oldPtr->effectIDs != newStack.ptr->effectIDs) - { - for (int i = 0; i < newStack.ptr->effectIDsCount; i++) - { - ref var element = ref newStack.ptr->effectIDs[i]; - - if (element.isSpatialEffect) - { - var ptr = m_spatialEffectIdToPtrMap[element.effectId].ptr; - element.requiresUpdateWhenCulled = ptr->requiresUpdateWhenCulled; - element.requiresUpdateWhenInputFrameDisconnected = ptr->requiresUpdateWhenInputFrameDisconnected; - } - else - { - var ptr = m_effectIdToPtrMap[element.effectId].ptr; - element.requiresUpdateWhenCulled = ptr->requiresUpdateWhenCulled; - element.requiresUpdateWhenInputFrameDisconnected = ptr->requiresUpdateWhenInputFrameDisconnected; - } - } - } + ref var meta = ref m_sourceStackIdToPtrMap[newStack.sourceId]; + meta.ptr->effectIDs = newStack.effectIDs; + meta.ptr->effectIDsCount = newStack.effectIDsCount; + meta.ptr->layerIndex = newStack.layerIndex; } } @@ -351,15 +316,6 @@ void ProcessCreatedListenerStacks(ref DspUpdateBuffer updateBuffer) { foreach (var newStack in updateBuffer.listenerStacksUpdateBuffer.newListenerStacks) { - newStack.ptr->hasVirtualOutput = false; - for (int i = 0; i < newStack.ptr->effectIDsCount; i++) - { - if (m_effectIdToPtrMap[newStack.ptr->effectIDs[i].effectId].ptr->isVirtualOutput) - { - newStack.ptr->hasVirtualOutput = true; - break; - } - } ref var state = ref m_listenerStackIdToStateMap[newStack.ptr->listenerId]; state = default; state.listenerMetadataPtr = newStack.ptr; @@ -376,29 +332,21 @@ void ProcessDestroyedListenerStacks(ref DspUpdateBuffer updateBuffer) } } - void ProcessFullyUpdatedListenerStacks(ref DspUpdateBuffer updateBuffer) + void ProcessUpdatedListenerStacks(ref DspUpdateBuffer updateBuffer) { foreach (var newStack in updateBuffer.listenerStacksUpdateBuffer.updatedListenerStacks) { - m_listenersDirty = true; - ref var storedStackState = ref m_listenerStackIdToStateMap[newStack.ptr->listenerId]; - var oldPtr = storedStackState.listenerMetadataPtr; - storedStackState.listenerMetadataPtr = newStack.ptr; - if (oldPtr->effectIDs != newStack.ptr->effectIDs) - { - newStack.ptr->hasVirtualOutput = false; - for (int i = 0; i < newStack.ptr->effectIDsCount; i++) - { - if (m_effectIdToPtrMap[newStack.ptr->effectIDs[i].effectId].ptr->isVirtualOutput) - { - newStack.ptr->hasVirtualOutput = true; - break; - } - } - } - else + m_listenersDirty = true; + ref var state = ref m_listenerStackIdToStateMap[newStack.listenerId]; + state.listenerMetadataPtr->effectIDs = newStack.effectIDs; + state.listenerMetadataPtr->effectIDsCount = newStack.effectIDsCount; + state.listenerMetadataPtr->hasVirtualOutput = newStack.hasVirtualOutput; + state.listenerMetadataPtr->layerMask = newStack.layerMask; + state.listenerMetadataPtr->limiterSettings = newStack.limiterSettings; + if (state.listenerMetadataPtr->listenerProfileBlob != newStack.listenerProfileBlob) { - newStack.ptr->hasVirtualOutput = oldPtr->hasVirtualOutput; + state.listenerMetadataPtr->listenerProfileBlob = newStack.listenerProfileBlob; + state.listenerMetadataPtr->listenerProfileFilters = newStack.listenerProfileFilters; } } } @@ -412,6 +360,131 @@ void ProcessTransformUpdatedListenerStacks(ref DspUpdateBuffer updateBuffer) } #endregion + #region Resources + void ProcessCreatedResourceComponents(ref DspUpdateBuffer buffer) + { + foreach (var newResource in buffer.resourcesUpdateBuffer.newComponentResources) + { + ref var metadata = ref m_resourceComponentIdToPtrMap[newResource.ptr->resourceComponentId]; + metadata = newResource; + m_resourceKeyToPtrMap.Add(new ResourceKey + { + componentType = newResource.ptr->resourceType, + entity = newResource.ptr->resourceEntity + }, + new ResourceValue { metadataPtr = metadata.ptr }); + } + } + + void ProcessUpdatedResourceComponents(ref DspUpdateBuffer buffer) + { + foreach (var updatedResource in buffer.resourcesUpdateBuffer.updatedComponentResources) + { + updatedResource.metadataPtr->componentPtr = updatedResource.newComponentPtr; + } + } + + void ProcessDestroyedResourceComponents(ref DspUpdateBuffer buffer) + { + foreach (var destroyedResource in buffer.resourcesUpdateBuffer.deadComponentResourceIDs) + { + ref var old = ref m_resourceComponentIdToPtrMap[destroyedResource]; + m_resourceKeyToPtrMap.Remove(new ResourceKey { componentType = old.ptr->resourceType, entity = old.ptr->resourceEntity }); + old = default; + } + } + + void ProcessCreatedResourceBuffers(ref DspUpdateBuffer buffer) + { + foreach (var newResource in buffer.resourcesUpdateBuffer.newBufferResources) + { + ref var metadata = ref m_resourceBufferIdToPtrMap[newResource.ptr->resourceBufferId]; + metadata = newResource; + m_resourceKeyToPtrMap.Add(new ResourceKey + { + componentType = newResource.ptr->resourceType, + entity = newResource.ptr->resourceEntity + }, + new ResourceValue { metadataPtr = metadata.ptr }); + } + } + + void ProcessUpdatedResourceBuffers(ref DspUpdateBuffer buffer) + { + foreach (var updatedResource in buffer.resourcesUpdateBuffer.updatedBufferResources) + { + updatedResource.metadataPtr->bufferPtr = updatedResource.newBufferPtr; + updatedResource.metadataPtr->elementCount = updatedResource.newElementCount; + } + } + + void ProcessDestroyedResourceBuffers(ref DspUpdateBuffer buffer) + { + foreach (var destroyedResource in buffer.resourcesUpdateBuffer.deadBufferResourceIDs) + { + ref var old = ref m_resourceBufferIdToPtrMap[destroyedResource]; + m_resourceKeyToPtrMap.Remove(new ResourceKey { componentType = old.ptr->resourceType, entity = old.ptr->resourceEntity }); + old = default; + } + } + #endregion + + #region Enabled States + void ProcessUpdatedEnabledStates(ref DspUpdateBuffer updateBuffer) + { + foreach (var updatedState in updateBuffer.enabledStatesUpdateBuffer.updatedEnabledStates) + { + switch (updatedState.type) + { + case EnabledStatusMetadataType.Effect: + { + var metadata = (EffectMetadata*)updatedState.metadataPtr; + metadata->enabled = updatedState.enabled; + break; + } + case EnabledStatusMetadataType.SpatialEffect: + { + var metadata = (SpatialEffectMetadata*)updatedState.metadataPtr; + metadata->enabled = updatedState.enabled; + break; + } + case EnabledStatusMetadataType.SourceStack: + { + var metadata = (SourceStackMetadata*)updatedState.metadataPtr; + metadata->enabled = updatedState.enabled; + break; + } + case EnabledStatusMetadataType.Listener: + { + var metadata = (ListenerStackMetadata*)updatedState.metadataPtr; + metadata->listenerEnabled = updatedState.enabled; + m_listenersDirty = true; + break; + } + case EnabledStatusMetadataType.ListenerStack: + { + var metadata = (ListenerStackMetadata*)updatedState.metadataPtr; + metadata->stackEnabled = updatedState.enabled; + m_listenersDirty = true; + break; + } + case EnabledStatusMetadataType.ResourceComponent: + { + var metadata = (ResourceComponentMetadata*)updatedState.metadataPtr; + metadata->enabled = updatedState.enabled; + break; + } + case EnabledStatusMetadataType.ResourceBuffer: + { + var metadata = (ResourceBufferMetadata*)updatedState.metadataPtr; + metadata->enabled = updatedState.enabled; + break; + } + } + } + } + #endregion + #region ListenersBatched void UpdateListenersWithPresampledChannels(ref DspUpdateBuffer updateBuffer) { @@ -447,6 +520,8 @@ void UpdateListenersLayers() ref var state = ref m_listenerStackIdToStateMap[i]; if (state.listenerMetadataPtr == null) continue; + if (!state.listenerMetadataPtr->listenerEnabled) + continue; BitField32 layerMask = default; layerMask.Value = state.listenerMetadataPtr->layerMask; @@ -461,6 +536,16 @@ void UpdateListenersLayers() } } #endregion + + #region Master + void ProcessMaster(ref DspUpdateBuffer dspUpdateBuffer) + { + m_masterLimiter.preGain = dspUpdateBuffer.masterLimiterSettings.preGain; + m_masterLimiter.limitDB = dspUpdateBuffer.masterLimiterSettings.limitDB; + m_masterLimiter.releasePerSampleDB = dspUpdateBuffer.masterLimiterSettings.releaseDBPerSample; + m_masterLimiter.SetLookaheadSampleCount(dspUpdateBuffer.masterLimiterSettings.lookaheadSampleCount); + } + #endregion } } diff --git a/MyriAudio/Internal/DspGraph/MyriMegaKernelSampleStacks.cs b/MyriAudio/Internal/DspGraph/MyriMegaKernelSampleStacks.cs index 18e8a3d..e348e0a 100644 --- a/MyriAudio/Internal/DspGraph/MyriMegaKernelSampleStacks.cs +++ b/MyriAudio/Internal/DspGraph/MyriMegaKernelSampleStacks.cs @@ -1,5 +1,4 @@ using System; -using Latios.Myri.DSP; using Latios.Myri.InternalSourceGen; using Latios.Transforms; using Unity.Collections; @@ -31,6 +30,8 @@ public void Dispose() void SampleSources() { + m_profilingSourceStacks.Begin(); + var preSplitFrame = m_framePool.Acquire(m_frameSize); var postSplitFrame = m_framePool.Acquire(m_frameSize); @@ -45,9 +46,13 @@ void SampleSources() updateContexts.updateContext.currentFrame = m_currentFrame; updateContexts.updateContext.stackType = StackType.Source; updateContexts.updateContext.virtualOutputsMap = m_entityToVirtualOutputMap; + updateContexts.updateContext.resourcesMap = m_resourceKeyToPtrMap; + + updateContexts.spatialCullingContext.resourcesMap = m_resourceKeyToPtrMap; updateContexts.spatialUpdateContext.currentFrame = m_currentFrame; updateContexts.spatialUpdateContext.virtualOutputsMap = m_entityToVirtualOutputMap; + updateContexts.spatialUpdateContext.resourcesMap = m_resourceKeyToPtrMap; for (int sourceStackId = 0; sourceStackId < m_sourceStackIdToPtrMap.length; sourceStackId++) { @@ -55,6 +60,8 @@ void SampleSources() if (sourcePtr == null) continue; ref var sourceStackMeta = ref *sourcePtr; + if (!sourceStackMeta.enabled) + continue; // Setup cull data var listeners = m_listenersByLayer[sourceStackMeta.layerIndex]; @@ -63,9 +70,7 @@ void SampleSources() m_samplingCache.cullList.AddReplicate(true, listeners.Length); // Setup state from source meta - updateContexts.updateContext.layerMask = 1u << sourceStackMeta.layerIndex; - updateContexts.updateContext.stackEntity = sourceStackMeta.sourceEntity; - updateContexts.updateContext.stackTransformPtr = (TransformQvvs*)UnsafeUtility.AddressOf(ref sourceStackMeta.worldTransform); + updateContexts.updateContext.metadataPtr = sourcePtr; updateContexts.spatialCullingContext.listeners = listeners; updateContexts.spatialCullingContext.sourcePtr = sourcePtr; @@ -78,7 +83,7 @@ void SampleSources() for (int i = 0; i < listeners.Length; i++) { var listener = listeners[i].ptr; - if (!listener->hasVirtualOutput && listener->limiterSettings.preGain == 0f) + if (!listener->hasVirtualOutput && listener->limiterSettings.preGain == 0f) // Disabled listeners aren't in the layer list updateContexts.cullArray.Cull(i); } @@ -88,10 +93,14 @@ void SampleSources() { if (sourceStackMeta.effectIDs[i].isSpatialEffect) { + ref var effectMeta = ref *m_spatialEffectIdToPtrMap[sourceStackMeta.effectIDs[i].effectId].ptr; + + if (!effectMeta.enabled) + continue; + if (firstSpatialIndex < 0) firstSpatialIndex = i; - ref var effectMeta = ref *m_spatialEffectIdToPtrMap[sourceStackMeta.effectIDs[i].effectId].ptr; updateContexts.effectContext.effectEntity = effectMeta.effectEntity; updateContexts.parametersPtr = effectMeta.parametersPtr; updateContexts.effectPtr = effectMeta.effectPtr; @@ -105,8 +114,11 @@ void SampleSources() preSplitFrame.frameIndex = m_currentFrame; updateContexts.sampleFramePtr = &preSplitFrame; - bool fullyCulled = updateContexts.cullArray.cullArray.Contains(true); - updateContexts.updateContext.isCulled = fullyCulled; + int aliveListenerCount = 0; + foreach (var alive in updateContexts.cullArray.cullArray) + aliveListenerCount += math.select(0, 1, alive); + bool fullyCulled = aliveListenerCount == 0; + updateContexts.updateContext.isCulled = fullyCulled; if (firstSpatialIndex < 0) firstSpatialIndex = sourceStackMeta.effectIDsCount; @@ -119,7 +131,11 @@ void SampleSources() if (!requiresUpdate) continue; - ref var effectMeta = ref *m_effectIdToPtrMap[effect.effectId].ptr; + ref var effectMeta = ref *m_effectIdToPtrMap[effect.effectId].ptr; + + if (!effectMeta.enabled) + continue; + updateContexts.effectContext.effectEntity = effectMeta.effectEntity; updateContexts.updateContext.indexInStack = i; updateContexts.parametersPtr = effectMeta.parametersPtr; @@ -142,12 +158,12 @@ void SampleSources() // Forward the pre-split sample frame ref var splitFrame = ref preSplitFrame; - if (listeners.Length != 1) + if (aliveListenerCount != 1 || isCulled) { splitFrame = ref postSplitFrame; splitFrame.frameIndex = m_currentFrame; updateContexts.sampleFramePtr = &postSplitFrame; - if (preSplitFrame.connected) + if (preSplitFrame.connected && !isCulled) { splitFrame.left.CopyFrom(preSplitFrame.left); splitFrame.right.CopyFrom(preSplitFrame.right); @@ -159,7 +175,7 @@ void SampleSources() } } - // Update post-splits + // Update post-split effects for (int i = firstSpatialIndex; i <= sourceStackMeta.effectIDsCount; i++) { var effect = sourceStackMeta.effectIDs[i]; @@ -170,7 +186,11 @@ void SampleSources() if (effect.isSpatialEffect) { - ref var effectMeta = ref *m_spatialEffectIdToPtrMap[effect.effectId].ptr; + ref var effectMeta = ref *m_spatialEffectIdToPtrMap[effect.effectId].ptr; + + if (!effectMeta.enabled) + continue; + updateContexts.effectContext.effectEntity = effectMeta.effectEntity; updateContexts.spatialUpdateContext.indexInStack = i; updateContexts.parametersPtr = effectMeta.parametersPtr; @@ -180,7 +200,11 @@ void SampleSources() } else { - ref var effectMeta = ref *m_effectIdToPtrMap[effect.effectId].ptr; + ref var effectMeta = ref *m_effectIdToPtrMap[effect.effectId].ptr; + + if (!effectMeta.enabled) + continue; + updateContexts.effectContext.effectEntity = effectMeta.effectEntity; updateContexts.updateContext.indexInStack = i; updateContexts.parametersPtr = effectMeta.parametersPtr; @@ -224,10 +248,14 @@ void SampleSources() m_framePool.Release(preSplitFrame); m_framePool.Release(postSplitFrame); + + m_profilingSourceStacks.End(); } void ProcessPresampledChannels() { + m_profilingSpatializers.Begin(); + ListenerState dummy = default; ref var listener = ref dummy; int listenerId = -1; @@ -248,8 +276,7 @@ void ProcessPresampledChannels() } if (listener.sampleFrame.frameIndex != m_currentFrame) { - listener.sampleFrame.left.AsSpan().Clear(); - listener.sampleFrame.right.AsSpan().Clear(); + listener.sampleFrame.ClearToZero(); listener.sampleFrame.frameIndex = m_currentFrame; } @@ -278,9 +305,86 @@ void ProcessPresampledChannels() } outSample += sample; } - output[sampleIndex] = outSample; + output[sampleIndex] += outSample; + } + } + + m_profilingSpatializers.End(); + } + + void SampleListeners() + { + m_profilingListenerStacks.Begin(); + + EffectOperations.CullUpdateOpData updateContexts = default; + updateContexts.effectContext.currentFrame = m_currentFrame; + updateContexts.effectContext.frameSize = m_frameSize; + updateContexts.effectContext.persistentAllocator = Allocator.AudioKernel; + updateContexts.effectContext.sampleFramePool = (SampleFramePool*)UnsafeUtility.AddressOf(ref m_framePool); + updateContexts.effectContext.sampleRate = m_sampleRate; + //updateContexts.effectContext.tempAllocator = m_rewindableAllocator.Allocator.Handle; + + updateContexts.updateContext.currentFrame = m_currentFrame; + updateContexts.updateContext.stackType = StackType.Listener; + updateContexts.updateContext.virtualOutputsMap = m_entityToVirtualOutputMap; + updateContexts.updateContext.resourcesMap = m_resourceKeyToPtrMap; + + for (int listenerIndex = 0; listenerIndex < m_listenerStackIdToStateMap.length; listenerIndex++) + { + ref var listener = ref m_listenerStackIdToStateMap[listenerIndex]; + if (listener.listenerMetadataPtr == null) + continue; + + ref var listenerMeta = ref *listener.listenerMetadataPtr; + if (!listenerMeta.listenerEnabled || !listenerMeta.stackEnabled) + continue; + + bool culled = !listenerMeta.hasVirtualOutput && listenerMeta.limiterSettings.preGain == 0f; + int effectCountNotCulled = listenerMeta.effectIDsCount; + if (listenerMeta.hasVirtualOutput && listenerMeta.limiterSettings.preGain == 0f) + { + do + { + effectCountNotCulled--; + var effect = listenerMeta.effectIDs[effectCountNotCulled]; + if (effect.isVirtualOutput) + break; + } + while (effectCountNotCulled >= 0); + effectCountNotCulled++; + } + + updateContexts.updateContext.metadataPtr = listener.listenerMetadataPtr; + updateContexts.sampleFramePtr = (SampleFrame*)UnsafeUtility.AddressOf(ref listener.sampleFrame); + + for (int i = 0; i <= listenerMeta.effectIDsCount; i++) + { + culled &= i < effectCountNotCulled; + + var effect = listenerMeta.effectIDs[i]; + bool requiresUpdate = effect.requiresUpdateWhenInputFrameDisconnected || listener.sampleFrame.connected; + requiresUpdate &= effect.requiresUpdateWhenCulled || culled; + if (!requiresUpdate) + continue; + + ref var effectMeta = ref *m_effectIdToPtrMap[effect.effectId].ptr; + + if (!effectMeta.enabled) + continue; + + if (culled) + updateContexts.sampleFramePtr->connected = false; + + updateContexts.effectContext.effectEntity = effectMeta.effectEntity; + updateContexts.updateContext.indexInStack = i; + updateContexts.parametersPtr = effectMeta.parametersPtr; + updateContexts.effectPtr = effectMeta.effectPtr; + + EffectOperations.UpdateEffect(ref updateContexts, effectMeta.functionPtr); } } + + m_profilingListenerStacks.End(); } } } diff --git a/MyriAudio/Internal/Interop/InteropStructures.cs b/MyriAudio/Internal/Interop/InteropStructures.cs index 4f505fb..fd2ff7d 100644 --- a/MyriAudio/Internal/Interop/InteropStructures.cs +++ b/MyriAudio/Internal/Interop/InteropStructures.cs @@ -36,6 +36,8 @@ internal unsafe struct DspUpdateBuffer public EffectsUpdateBuffer effectsUpdateBuffer; public SourceStacksUpdateBuffer sourceStacksUpdateBuffer; public ListenerStacksUpdateBuffer listenerStacksUpdateBuffer; + public ResourcesUpdateBuffer resourcesUpdateBuffer; + public EnabledStatesUpdateBuffer enabledStatesUpdateBuffer; public BrickwallLimiterSettings masterLimiterSettings; } @@ -86,11 +88,10 @@ internal unsafe struct EffectMetadata public void* parametersPtr; public void* effectPtr; public int effectId; - - // Written in DSP Thread - public bool requiresUpdateWhenCulled; - public bool requiresUpdateWhenInputFrameDisconnected; - public bool isVirtualOutput; + public bool enabled; + public bool requiresUpdateWhenCulled; + public bool requiresUpdateWhenInputFrameDisconnected; + public bool isVirtualOutput; internal unsafe struct Ptr { @@ -106,10 +107,9 @@ internal unsafe struct SpatialEffectMetadata public void* parametersPtr; public void* effectPtr; public int effectId; - - // Written in DSP Thread - public bool requiresUpdateWhenCulled; - public bool requiresUpdateWhenInputFrameDisconnected; + public bool enabled; + public bool requiresUpdateWhenCulled; + public bool requiresUpdateWhenInputFrameDisconnected; internal unsafe struct Ptr { @@ -129,10 +129,9 @@ internal struct EffectIDInStack { public int effectId; public bool isSpatialEffect; - - // Written in DSP Thread public bool requiresUpdateWhenCulled; public bool requiresUpdateWhenInputFrameDisconnected; + public bool isVirtualOutput; } internal unsafe struct StackTransformUpdate @@ -146,7 +145,7 @@ internal unsafe struct StackTransformUpdate internal unsafe struct SourceStacksUpdateBuffer { public UnsafeList newSourceStacks; - public UnsafeList updatedSourceStacks; + public UnsafeList updatedSourceStacks; public UnsafeList updatedSourceStackTransforms; public UnsafeList deadSourceStackIDs; } @@ -159,6 +158,7 @@ internal unsafe struct SourceStackMetadata public int sourceId; public int effectIDsCount; public byte layerIndex; + public bool enabled; internal unsafe struct Ptr { @@ -166,13 +166,21 @@ internal unsafe struct Ptr } } + internal unsafe struct SourceStackUpdate + { + public EffectIDInStack* effectIDs; + public int sourceId; + public int effectIDsCount; + public byte layerIndex; + } + #endregion #region Listener Stacks internal struct ListenerStacksUpdateBuffer { public UnsafeList newListenerStacks; - public UnsafeList updatedListenerStacks; + public UnsafeList updatedListenerStacks; public UnsafeList updatedListenerStackTransforms; public UnsafeList deadListenerStackIDs; } @@ -184,15 +192,13 @@ internal unsafe struct ListenerStackMetadata public BlobAssetReference listenerProfileBlob; public Entity listenerEntity; public EffectIDInStack* effectIDs; - public ListenerPropertyPtr* listenerProperties; public DSP.StateVariableFilter.Channel* listenerProfileFilters; public int listenerId; public int effectIDsCount; - public int listenerPropertiesCount; public uint layerMask; - - // Written by DSP Thread - public bool hasVirtualOutput; + public bool listenerEnabled; + public bool stackEnabled; + public bool hasVirtualOutput; internal unsafe struct Ptr { @@ -200,10 +206,95 @@ internal unsafe struct Ptr } } - internal unsafe struct ListenerPropertyPtr + internal unsafe struct ListenerStackUpdate + { + public BrickwallLimiterSettings limiterSettings; + public BlobAssetReference listenerProfileBlob; + public EffectIDInStack* effectIDs; + public DSP.StateVariableFilter.Channel* listenerProfileFilters; + public int listenerId; + public int effectIDsCount; + public uint layerMask; + public bool hasVirtualOutput; + } + #endregion + + #region Resources + internal struct ResourcesUpdateBuffer + { + public UnsafeList newComponentResources; + public UnsafeList updatedComponentResources; + public UnsafeList deadComponentResourceIDs; + public UnsafeList newBufferResources; + public UnsafeList updatedBufferResources; + public UnsafeList deadBufferResourceIDs; + } + + internal unsafe struct ResourceComponentMetadata + { + public void* componentPtr; + public Entity resourceEntity; + public ComponentType resourceType; + public int resourceComponentId; + public bool enabled; + + internal unsafe struct Ptr + { + public ResourceComponentMetadata* ptr; + } + } + + internal unsafe struct ResourceComponentUpdate + { + public ResourceComponentMetadata* metadataPtr; + public void* newComponentPtr; + } + + internal unsafe struct ResourceBufferMetadata + { + public void* bufferPtr; + public Entity resourceEntity; + public ComponentType resourceType; + public int resourceBufferId; + public int elementCount; + public bool enabled; + + internal unsafe struct Ptr + { + public ResourceBufferMetadata* ptr; + } + } + + internal unsafe struct ResourceBufferUpdate + { + public ResourceBufferMetadata* metadataPtr; + public void* newBufferPtr; + public int newElementCount; + } + #endregion + + #region EnabledStatuses + internal struct EnabledStatesUpdateBuffer + { + public UnsafeList updatedEnabledStates; + } + + internal enum EnabledStatusMetadataType + { + Effect, + SpatialEffect, + SourceStack, + Listener, + ListenerStack, + ResourceComponent, + ResourceBuffer, + } + + internal unsafe struct EnabledStatusUpdate { - public void* propertyPtr; - public ComponentType propertyType; + public void* metadataPtr; + public EnabledStatusMetadataType type; + public bool enabled; } #endregion } diff --git a/MyriAudio/Internal/SourceGen/SourceGenDispatchers.cs b/MyriAudio/Internal/SourceGen/SourceGenDispatchers.cs index b6a9dc0..8932233 100644 --- a/MyriAudio/Internal/SourceGen/SourceGenDispatchers.cs +++ b/MyriAudio/Internal/SourceGen/SourceGenDispatchers.cs @@ -14,9 +14,6 @@ public struct InitDestroyOpData public DSP.EffectContext effectContext; public void* parametersPtr; public void* effectPtr; - public bool requiresUpdateWhenCulled; - public bool requiresUpdateWhenInputFrameDisconnected; - public bool isVirtualOutput; } public static void InitEffect(ref InitDestroyOpData data, FunctionPointer functionPtr) diff --git a/PsyshockPhysics/Physics/Authoring/CompoundColliderSmartBlobberSystem.cs b/PsyshockPhysics/Physics/Authoring/CompoundColliderSmartBlobberSystem.cs index ec9bca5..3d09502 100644 --- a/PsyshockPhysics/Physics/Authoring/CompoundColliderSmartBlobberSystem.cs +++ b/PsyshockPhysics/Physics/Authoring/CompoundColliderSmartBlobberSystem.cs @@ -5,6 +5,7 @@ using Latios.Transforms.Authoring.Abstract; using Unity.Burst; using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; using Unity.Entities; using Unity.Entities.LowLevel.Unsafe; using Unity.Jobs; @@ -143,14 +144,22 @@ public void OnUpdate(ref SystemState state) [WithOptions(EntityQueryOptions.IncludePrefab | EntityQueryOptions.IncludeDisabledEntities)] partial struct Job : IJobEntity { + UnsafeList childPropertiesCache; + public void Execute(ref SmartBlobberResult result, in DynamicBuffer buffer) { + if (!childPropertiesCache.IsCreated) + childPropertiesCache = new UnsafeList(buffer.Length, Allocator.Temp); + childPropertiesCache.Clear(); + var builder = new BlobBuilder(Allocator.Temp); ref var root = ref builder.ConstructRoot(); var blobColliders = builder.Allocate(ref root.colliders, buffer.Length); var blobTransforms = builder.Allocate(ref root.transforms, buffer.Length); - Aabb aabb = new Aabb(float.MaxValue, float.MinValue); + Aabb aabb = new Aabb(float.MaxValue, float.MinValue); + float3 combinedCenterOfMass = float3.zero; + float combinedVolume = 0f; for (int i = 0; i < buffer.Length; i++) { blobColliders[i] = Physics.ScaleStretchCollider(buffer[i].collider, buffer[i].transform.scale, buffer[i].transform.stretch); @@ -158,10 +167,108 @@ public void Execute(ref SmartBlobberResult result, in DynamicBuffer 0f) + combinedCenterOfMass /= combinedVolume; + + var combinedInertiaMatrix = float3x3.zero; + foreach (var child in childPropertiesCache) + { + // shift the inertia to be relative to the new center of mass + float3 shift = child.centerOfMassInCompound - combinedCenterOfMass; + float3 shiftSq = shift * shift; + var diag = new float3(shiftSq.y + shiftSq.z, shiftSq.x + shiftSq.z, shiftSq.x + shiftSq.y); + var offDiag = new float3(shift.x * shift.y, shift.y * shift.z, shift.z * shift.x) * -1.0f; + var inertiaMatrix = child.inertiaMatrixUnshifted; + inertiaMatrix.c0 += new float3(diag.x, offDiag.x, offDiag.z); + inertiaMatrix.c1 += new float3(offDiag.x, diag.y, offDiag.y); + inertiaMatrix.c2 += new float3(offDiag.z, offDiag.y, diag.z); + + // weight by its proportional volume (=mass) + inertiaMatrix *= child.volume / (combinedVolume + float.Epsilon); + combinedInertiaMatrix += inertiaMatrix; } - root.localAabb = aabb; - result.blob = UnsafeUntypedBlobAssetReference.Create(builder.CreateBlobAssetReference(Allocator.Persistent)); + root.localAabb = aabb; + root.centerOfMass = combinedCenterOfMass; + root.inertiaTensor = combinedInertiaMatrix; + mathex.DiagonalizeSymmetricApproximation(root.inertiaTensor, out var inertiaTensorOrientation, out root.unscaledInertiaTensorDiagonal); + root.unscaledInertiaTensorOrientation = new quaternion(inertiaTensorOrientation); + + result.blob = UnsafeUntypedBlobAssetReference.Create(builder.CreateBlobAssetReference(Allocator.Persistent)); + } + + struct ChildProperties + { + public float3x3 inertiaMatrixUnshifted; + public float3 centerOfMassInCompound; + public float volume; } } } diff --git a/PsyshockPhysics/Physics/Authoring/ConvexColliderSmartBlobberSystem.cs b/PsyshockPhysics/Physics/Authoring/ConvexColliderSmartBlobberSystem.cs index 73a0a7b..ad4cdc6 100644 --- a/PsyshockPhysics/Physics/Authoring/ConvexColliderSmartBlobberSystem.cs +++ b/PsyshockPhysics/Physics/Authoring/ConvexColliderSmartBlobberSystem.cs @@ -298,7 +298,7 @@ public unsafe void BuildBlob(Mesh.MeshData mesh) facePlaneX[faceIndex] = plane.normal.x; facePlaneY[faceIndex] = plane.normal.y; facePlaneZ[faceIndex] = plane.normal.z; - facePlaneDist[faceIndex] = plane.distanceFromOrigin; + facePlaneDist[faceIndex] = plane.distanceToOrigin; var edgeIndicesStartAndCount = edgeIndicesInFacesStartsAndCounts[faceIndex]; @@ -371,7 +371,14 @@ public unsafe void BuildBlob(Mesh.MeshData mesh) runningCount += fibvsac[vertexIndex].count; } - blobRoot.localAabb = aabb; + if (convexHullBuilder.hullMassProperties.volume == 0f) + convexHullBuilder.UpdateHullMassProperties(); + + blobRoot.localAabb = aabb; + blobRoot.centerOfMass = convexHullBuilder.hullMassProperties.centerOfMass; + blobRoot.inertiaTensor = convexHullBuilder.hullMassProperties.inertiaTensor; + mathex.DiagonalizeSymmetricApproximation(blobRoot.inertiaTensor, out var inertiaTensorOrientation, out blobRoot.unscaledInertiaTensorDiagonal); + blobRoot.unscaledInertiaTensorOrientation = new quaternion(inertiaTensorOrientation); var fibv = builder.Allocate(ref blobRoot.faceIndicesByVertex, runningCount); var fibvCounts = new NativeArray(vertices.Length, Allocator.Temp, NativeArrayOptions.ClearMemory); diff --git a/PsyshockPhysics/Physics/Authoring/TriMeshColliderSmartBlobberSystem.cs b/PsyshockPhysics/Physics/Authoring/TriMeshColliderSmartBlobberSystem.cs index 068ba62..80da6ed 100644 --- a/PsyshockPhysics/Physics/Authoring/TriMeshColliderSmartBlobberSystem.cs +++ b/PsyshockPhysics/Physics/Authoring/TriMeshColliderSmartBlobberSystem.cs @@ -197,11 +197,11 @@ public unsafe void BuildBlob(Mesh.MeshData mesh) Physics.BuildCollisionLayer(bodies.AsArray()).WithSubdivisions(1).RunImmediate(out var layer, Allocator.Temp); - builder.ConstructFromNativeArray(ref blobRoot.xmins, layer.xmins); - builder.ConstructFromNativeArray(ref blobRoot.xmaxs, layer.xmaxs); - builder.ConstructFromNativeArray(ref blobRoot.yzminmaxs, layer.yzminmaxs); - builder.ConstructFromNativeArray(ref blobRoot.intervalTree, layer.intervalTrees); - builder.ConstructFromNativeArray(ref blobRoot.sourceIndices, layer.srcIndices); + builder.ConstructFromNativeArray(ref blobRoot.xmins, layer.xmins.AsArray()); + builder.ConstructFromNativeArray(ref blobRoot.xmaxs, layer.xmaxs.AsArray()); + builder.ConstructFromNativeArray(ref blobRoot.yzminmaxs, layer.yzminmaxs.AsArray()); + builder.ConstructFromNativeArray(ref blobRoot.intervalTree, layer.intervalTrees.AsArray()); + builder.ConstructFromNativeArray(ref blobRoot.sourceIndices, layer.srcIndices.AsArray()); var triangles = builder.Allocate(ref blobRoot.triangles, layer.count); var aabb = new Aabb(float.MaxValue, float.MinValue); diff --git a/PsyshockPhysics/Physics/Components/ColliderPsyshock.cs b/PsyshockPhysics/Physics/Components/ColliderPsyshock.cs index 0901a5e..7b5907e 100644 --- a/PsyshockPhysics/Physics/Components/ColliderPsyshock.cs +++ b/PsyshockPhysics/Physics/Components/ColliderPsyshock.cs @@ -1,6 +1,9 @@ using System; using System.Diagnostics; using System.Runtime.InteropServices; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; using Unity.Entities; using Unity.Mathematics; @@ -74,28 +77,30 @@ public unsafe struct Collider : IComponentData internal ConvexCollider m_convex; // Unity crashes when there are aliased BlobAssetReferences. - // So we reinterpret the pointer instead. - internal ref TriMeshCollider m_triMesh - { - get - { - TriMeshCollider* ret; - fixed (void* ptr = &m_convex) - ret = (TriMeshCollider*)ptr; - return ref *ret; - } - } - - internal ref CompoundCollider m_compound - { - get - { - CompoundCollider* ret; - fixed (void* ptr = &m_convex) - ret = (CompoundCollider*)ptr; - return ref *ret; - } - } + // So we have to use ColliderBlobHelpers instead. + //internal ref TriMeshCollider m_triMesh => ref ColliderBlobHelpers.AsTriMesh(ref m_convex); + //{ + // get + // { + // UnsafeUtility.As() + // + // TriMeshCollider* ret; + // fixed (void* ptr = &m_convex) + // ret = (TriMeshCollider*)ptr; + // return ref *ret; + // } + //} + + //internal ref CompoundCollider m_compound + //{ + // get + // { + // CompoundCollider* ret; + // fixed (void* ptr = &m_convex) + // ret = (CompoundCollider*)ptr; + // return ref *ret; + // } + //} private struct Storage { @@ -180,30 +185,30 @@ public static implicit operator ConvexCollider(Collider collider) public static implicit operator Collider(TriMeshCollider triMeshCollider) { - Collider collider = default; - collider.m_type = ColliderType.TriMesh; - collider.m_triMesh = triMeshCollider; + Collider collider = default; + collider.m_type = ColliderType.TriMesh; + collider.m_triMeshRW() = triMeshCollider; return collider; } public static implicit operator TriMeshCollider(Collider collider) { CheckColliderIsCastTargetType(in collider, ColliderType.TriMesh); - return collider.m_triMesh; + return collider.m_triMesh(); } public static implicit operator Collider(CompoundCollider compoundCollider) { - Collider collider = default; - collider.m_type = ColliderType.Compound; - collider.m_compound = compoundCollider; + Collider collider = default; + collider.m_type = ColliderType.Compound; + collider.m_compoundRW() = compoundCollider; return collider; } public static implicit operator CompoundCollider(Collider collider) { CheckColliderIsCastTargetType(in collider, ColliderType.Compound); - return collider.m_compound; + return collider.m_compound(); } [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] @@ -227,5 +232,15 @@ internal static void CheckColliderIsCastTargetType(in Collider c, ColliderType t } #endregion } + + internal static class ColliderBlobHelpers + { + public static ref TriMeshCollider m_triMeshRW(ref this Collider collider) => ref UnsafeUtility.As(ref collider.m_convex); + public static ref CompoundCollider m_compoundRW(ref this Collider collider) => ref UnsafeUtility.As(ref collider.m_convex); + public static ref TriMeshCollider m_triMesh(in this Collider collider) => ref UnsafeUtility.As(ref UnsafeUtilityExtensions.AsRef(in collider.m_convex)); + public static ref CompoundCollider m_compound(in this Collider collider) => ref UnsafeUtility.As(ref UnsafeUtilityExtensions.AsRef(in collider.m_convex)); + } } diff --git a/PsyshockPhysics/Physics/Debug/PhysicsDebug.DrawCollider.cs b/PsyshockPhysics/Physics/Debug/PhysicsDebug.DrawCollider.cs index c3c3653..71196fa 100644 --- a/PsyshockPhysics/Physics/Debug/PhysicsDebug.DrawCollider.cs +++ b/PsyshockPhysics/Physics/Debug/PhysicsDebug.DrawCollider.cs @@ -21,7 +21,7 @@ public static void DrawCollider(in SphereCollider sphere, in RigidTransform tran float2 previous = new float2(1f, 0f); var tf = transform; - tf.pos += sphere.center; + tf.pos += math.rotate(transform, sphere.center); for (int segment = 0; segment < segmentsPerPi; segment++) { @@ -262,25 +262,25 @@ public static void DrawCollider(in Collider collider, in RigidTransform transfor switch (collider.type) { case ColliderType.Sphere: - DrawCollider(in collider.m_sphere, transform, color, segmentsPerPi); + DrawCollider(in collider.m_sphere, transform, color, segmentsPerPi); break; case ColliderType.Capsule: - DrawCollider(in collider.m_capsule, transform, color, segmentsPerPi); + DrawCollider(in collider.m_capsule, transform, color, segmentsPerPi); break; case ColliderType.Box: - DrawCollider(in collider.m_box, transform, color); + DrawCollider(in collider.m_box, transform, color); break; case ColliderType.Triangle: - DrawCollider(in collider.m_triangle, transform, color); + DrawCollider(in collider.m_triangle, transform, color); break; case ColliderType.Convex: - DrawCollider(in collider.m_convex, transform, color); + DrawCollider(in collider.m_convex, transform, color); break; case ColliderType.TriMesh: - DrawCollider(in collider.m_triMesh, transform, color); + DrawCollider(in collider.m_triMesh(), transform, color); break; case ColliderType.Compound: - DrawCollider(in collider.m_compound, transform, color, segmentsPerPi); + DrawCollider(in collider.m_compound(), transform, color, segmentsPerPi); break; } } diff --git a/PsyshockPhysics/Physics/Debug/PhysicsDebug.LogFindPairsStats.cs b/PsyshockPhysics/Physics/Debug/PhysicsDebug.LogFindPairsStats.cs index 39ab791..12cc8d4 100644 --- a/PsyshockPhysics/Physics/Debug/PhysicsDebug.LogFindPairsStats.cs +++ b/PsyshockPhysics/Physics/Debug/PhysicsDebug.LogFindPairsStats.cs @@ -65,30 +65,6 @@ public JobHandle Schedule(JobHandle inputDeps) } } - internal static JobHandle LogFindPairsStats(CollisionLayer layer, FixedString128Bytes layerName, JobHandle inputDeps) - { - return new FindPairsLayerSelfStatsJob - { - layer = layer, - layerName = layerName - }.ScheduleParallel(layer.bucketCount * 2 - 1, 1, inputDeps); - } - - internal static JobHandle LogFindPairsStats(CollisionLayer layerA, - FixedString128Bytes layerNameA, - CollisionLayer layerB, - FixedString128Bytes layerNameB, - JobHandle inputDeps) - { - return new FindPairsLayerLayerStatsJob - { - layerA = layerA, - layerNameA = layerNameA, - layerB = layerB, - layerNameB = layerNameB - }.ScheduleParallel(3 * layerA.bucketCount - 2, 1, inputDeps); - } - [BurstCompile] struct LogBucketCountsForLayerJob : IJob { @@ -127,63 +103,6 @@ public void Execute() UnityEngine.Debug.Log($"Colliders counts per bucket in layer {layerName}:\n{countsAsString}"); } } - - [BurstCompile] - struct FindPairsLayerSelfStatsJob : IJobFor - { - [ReadOnly] public CollisionLayer layer; - public FixedString128Bytes layerName; - - public void Execute(int index) - { - if (index < layer.bucketCount) - { - var bucket = layer.GetBucketSlices(index); - FindPairsSweepMethods.SelfSweepStats(bucket, in layerName); - } - else - { - index -= layer.bucketCount; - var bucket = layer.GetBucketSlices(index); - var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); - FindPairsSweepMethods.BipartiteSweepStats(bucket, in layerName, crossBucket, in layerName); - } - } - } - - [BurstCompile] - struct FindPairsLayerLayerStatsJob : IJobFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public FixedString128Bytes layerNameA; - public FixedString128Bytes layerNameB; - - public void Execute(int i) - { - if (i < layerA.bucketCount) - { - var bucketA = layerA.GetBucketSlices(i); - var bucketB = layerB.GetBucketSlices(i); - FindPairsSweepMethods.BipartiteSweepStats(bucketA, layerNameA, bucketB, layerNameB); - } - else if (i < 2 * layerB.bucketCount - 1) - { - i -= layerB.bucketCount; - var bucket = layerB.GetBucketSlices(i); - var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); - FindPairsSweepMethods.BipartiteSweepStats(crossBucket, layerNameA, bucket, layerNameB); - } - else - { - var jobIndex = i; - i -= (2 * layerB.bucketCount - 1); - var bucket = layerA.GetBucketSlices(i); - var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); - FindPairsSweepMethods.BipartiteSweepStats(bucket, layerNameA, crossBucket, layerNameB); - } - } - } } } diff --git a/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContactJacobians.cs b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContactJacobians.cs new file mode 100644 index 0000000..8d07ad1 --- /dev/null +++ b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContactJacobians.cs @@ -0,0 +1,569 @@ +using System; +using System.Diagnostics; +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; + +namespace Latios.Psyshock +{ + public static partial class UnitySim + { + /// A contact jacobian angular. + public struct ContactJacobianAngular + { + /// The angular a. + public float3 angularA; + /// The angular b. + public float3 angularB; + /// The effective mass. + public float effectiveMass; + } + + /// A contact jacobian angle and velocity for a contact point to reach the contact plane. + public struct ContactJacobianContactParameters + { + /// The jacobian. + public ContactJacobianAngular jacobianAngular; + + /// + /// Velocity needed to reach the contact plane in one frame, both if approaching (negative) and + /// depenetrating (positive) + /// + public float velocityToReachContactPlane; + } + + /// + /// Contact body friction and surface normal values. + /// + public struct ContactJacobianBodyParameters + { + // Todo: Better understand these and document them. + + // Linear friction + public ContactJacobianAngular friction0; // effectiveMass stores friction effective mass matrix element (0, 0) + public ContactJacobianAngular friction1; // effectiveMass stores friction effective mass matrix element (1, 1) + public float3 frictionDirection0; + public float3 frictionDirection1; + + // Angular friction about the contact normal, no linear part + public ContactJacobianAngular angularFriction; // effectiveMass stores friction effective mass matrix element (2, 2) + public float3 frictionEffectiveMassOffDiag; // Effective mass matrix (0, 1), (0, 2), (1, 2) == (1, 0), (2, 0), (2, 1) + + public float3 contactNormal; + public float3 surfaceVelocityDv; + + public float coefficientOfFriction; + + public void SetSurfaceVelocity(Velocity surfaceVelocity) + { + surfaceVelocityDv = default; + if (!surfaceVelocity.Equals(float3.zero)) + { + float linVel0 = math.dot(surfaceVelocity.linear, frictionDirection0); + float linVel1 = math.dot(surfaceVelocity.linear, frictionDirection1); + + float angVelProj = math.dot(surfaceVelocity.angular, contactNormal); + surfaceVelocityDv = new float3(linVel0, linVel1, angVelProj); + } + } + } + + /// + /// Per-body stabilization state + /// + public struct MotionStabilizationInput + { + public Velocity inputVelocity; + public float inverseInertiaScale; + + public static readonly MotionStabilizationInput kDefault = new MotionStabilizationInput + { + inputVelocity = default, + inverseInertiaScale = 1.0f + }; + } + + /// + /// Impulses generated by the contact during a SolveJacobian call + /// + public struct ContactJacobianImpulses + { + /// + /// The magnitude of the impulse applied by all contact points + /// + public float combinedContactPointsImpulse; + /// + /// The impulse from friction applied in the primary friction axis + /// + public float friction0Impulse; + /// + /// The impulse from friction applied in the secondary friction axis + /// + public float friction1Impulse; + /// + /// The impulse from friction applied via rotational friction about the contact + /// + public float frictionAngularImpulse; + } + + /// + /// Unity's default maximum depenetration velocity between a dynamic and static object + /// + public const float kMaxDepenetrationVelocityDynamicStatic = float.MaxValue; + /// + /// Unity's default maximum depenetration velocity between two dynamic objects + /// + public const float kMaxDepenetrationVelocityDynamicDynamic = 3f; + + // Notes: In Unity Physics, inertialPoseWorldTransformA/B is identity if the body is static. + // perContactParameters can be uninitialized. + // gravityAgainstContactNormal uses the magnitude of global gravity. Would a dot product with the contact normal be more appropriate? + /// + /// Construct ContactJacobianBodyParameters for the pair of potentially contacting bodies and ContactJacobianContactParameters per contact point. + /// This primes data for faster solving in a solver loop. + /// + /// Parameters per contact that should be generated to avoid recomputation in the solver loop. + /// The elements of the span can be uninitialized memory. + /// Parameters for the pair that should be generated to avoid recomputation in the solver loop + /// The world-space transform of the center of mass and inertia tensor diagonal orientation for the first body. + /// Can be identity for a static body. + /// The velocity of the first body. Can be default for a static body. + /// The mass of the first body. Can be default for a static body. + /// The world-space transform of the center of mass and inertia tensor diagonal orientation for the second body. + /// Can be identity for a static body. + /// The velocity of the second body. Can be default for a static body. + /// The mass of the second body. Can be default for a static body. + /// The contact normal that points from collider B to collider A (when no penetration is present) + /// The list of contact points which lie on the surface of collider B + /// The bounciness of the contact in the range [0, 1] + /// The amount of "grip" between the surfaces in the range [0, 1] + /// The amount of velocity to be added to depenetrate bodies that are initially overlapping + /// The maximum amount of gravity being applied opposite to the contact normal. + /// You can simply use the magnitude of the gravity vector for this. + /// The timestep of this physics step + /// The inverse of the timestep for this physics step + public static void BuildJacobian(Span perContactParameters, out ContactJacobianBodyParameters bodyParameters, + RigidTransform inertialPoseWorldTransformA, in Velocity velocityA, in Mass massA, + RigidTransform inertialPoseWorldTransformB, in Velocity velocityB, in Mass massB, + float3 contactNormal, ReadOnlySpan contacts, + float coefficientOfRestitution, float coefficientOfFriction, + float maxDepenetrationVelocity, float gravityAgainstContactNormal, + float deltaTime, float inverseDeltaTime) + { + CheckContactAndJacobianSpanLengthsEqual(perContactParameters.Length, contacts.Length); + + var negContactRestingVelocity = -gravityAgainstContactNormal * deltaTime; + var sumInverseMasses = massA.inverseMass + massB.inverseMass; + var inverseRotationA = math.conjugate(inertialPoseWorldTransformA.rot); + var inverseRotationB = math.conjugate(inertialPoseWorldTransformB.rot); + float3 centerA = 0f; + float3 centerB = 0f; + + // Indicator whether restitution will be applied, + // used to scale down friction on bounce. + bool applyRestitution = false; + + for (int i = 0; i < perContactParameters.Length; i++) + { + // Build the jacobian + ref var jacAngular = ref perContactParameters[i]; + var contact = contacts[i]; + float3 pointOnB = contact.location; + float3 pointOnA = contact.location + contactNormal * contact.distanceToA; + float3 armA = pointOnA - inertialPoseWorldTransformA.pos; + float3 armB = pointOnB - inertialPoseWorldTransformB.pos; + BuildJacobianAngular(inverseRotationA, inverseRotationB, contactNormal, armA, armB, massA.inverseInertia, massB.inverseInertia, sumInverseMasses, + out jacAngular.jacobianAngular.angularA, out jacAngular.jacobianAngular.angularB, out float invEffectiveMass); + jacAngular.jacobianAngular.effectiveMass = 1.0f / invEffectiveMass; + + float solveDistance = contact.distanceToA; + float solveVelocity = solveDistance * inverseDeltaTime; + + solveVelocity = math.max(-maxDepenetrationVelocity, solveVelocity); + + jacAngular.velocityToReachContactPlane = -solveVelocity; + + // Calculate average position for friction + centerA += armA; + centerB += armB; + + // Restitution (optional) + if (coefficientOfRestitution > 0.0f) + { + float relativeVelocity = GetJacVelocity(contactNormal, jacAngular.jacobianAngular, + velocityA.linear, velocityA.angular, velocityB.linear, velocityB.angular); + float dv = jacAngular.velocityToReachContactPlane - relativeVelocity; + if (dv > 0.0f && relativeVelocity < negContactRestingVelocity) + { + // Note: The following comment comes from Unity Physics. However, gravityAcceleration was renamed to + // gravityAgainstContactNormal. + + // Restitution impulse is applied as if contact point is on the contact plane. + // However, it can (and will) be slightly away from contact plane at the moment restitution is applied. + // So we have to apply vertical shot equation to make sure we don't gain energy: + // effectiveRestitutionVelocity^2 = restitutionVelocity^2 - 2.0f * gravityAcceleration * distanceToGround + // From this formula we calculate the effective restitution velocity, which is the velocity + // that the contact point needs to reach the same height from current position + // as if it was shot with the restitutionVelocity from the contact plane. + // ------------------------------------------------------------ + // This is still an approximation for 2 reasons: + // - We are assuming the contact point will hit the contact plane with its current velocity, + // while actually it would have a portion of gravity applied before the actual hit. However, + // that velocity increase is quite small (less than gravity in one step), so it's safe + // to use current velocity instead. + // - gravityAcceleration is the actual value of gravity applied only when contact plane is + // directly opposite to gravity direction. Otherwise, this value will only be smaller. + // However, since this can only result in smaller bounce than the "correct" one, we can + // safely go with the default gravity value in all cases. + float restitutionVelocity = (relativeVelocity - negContactRestingVelocity) * coefficientOfRestitution; + float distanceToGround = math.max(-jacAngular.velocityToReachContactPlane * deltaTime, 0.0f); + float effectiveRestitutionVelocity = + math.sqrt(math.max(restitutionVelocity * restitutionVelocity - 2.0f * gravityAgainstContactNormal * distanceToGround, 0.0f)); + + jacAngular.velocityToReachContactPlane = + math.max(jacAngular.velocityToReachContactPlane - effectiveRestitutionVelocity, 0.0f) + + effectiveRestitutionVelocity; + + // Remember that restitution should be applied + applyRestitution = true; + } + } + } + + // Build friction jacobians + { + // Clear accumulated impulse + bodyParameters = default; + bodyParameters.coefficientOfFriction = coefficientOfFriction; + bodyParameters.contactNormal = contactNormal; + + // Calculate average position + float invNumContacts = math.rcp(contacts.Length); + centerA *= invNumContacts; + centerB *= invNumContacts; + + // Choose friction axes + mathex.GetDualPerpendicularNormalized(contactNormal, out float3 frictionDir0, out float3 frictionDir1); + bodyParameters.frictionDirection0 = frictionDir0; + bodyParameters.frictionDirection1 = frictionDir1; + + // Build linear jacobian + float invEffectiveMass0, invEffectiveMass1; + { + float3 armA = centerA; + float3 armB = centerB; + BuildJacobianAngular(inverseRotationA, inverseRotationB, frictionDir0, armA, armB, massA.inverseInertia, massB.inverseInertia, sumInverseMasses, + out bodyParameters.friction0.angularA, out bodyParameters.friction0.angularB, out invEffectiveMass0); + BuildJacobianAngular(inverseRotationA, inverseRotationB, frictionDir1, armA, armB, massA.inverseInertia, massB.inverseInertia, sumInverseMasses, + out bodyParameters.friction1.angularA, out bodyParameters.friction1.angularB, out invEffectiveMass1); + } + + // Build angular jacobian + float invEffectiveMassAngular; + { + bodyParameters.angularFriction.angularA = math.mul(inverseRotationA, contactNormal); + bodyParameters.angularFriction.angularB = math.mul(inverseRotationB, -contactNormal); + float3 temp = bodyParameters.angularFriction.angularA * bodyParameters.angularFriction.angularA * massA.inverseInertia; + temp += bodyParameters.angularFriction.angularB * bodyParameters.angularFriction.angularB * massB.inverseInertia; + invEffectiveMassAngular = math.csum(temp); + } + + // Build effective mass + { + // Build the inverse effective mass matrix + var invEffectiveMassDiag = new float3(invEffectiveMass0, invEffectiveMass1, invEffectiveMassAngular); + var invEffectiveMassOffDiag = new float3( // (0, 1), (0, 2), (1, 2) + CalculateInvEffectiveMassOffDiag(bodyParameters.friction0.angularA, bodyParameters.friction1.angularA, massA.inverseInertia, + bodyParameters.friction0.angularB, bodyParameters.friction1.angularB, massB.inverseInertia), + CalculateInvEffectiveMassOffDiag(bodyParameters.friction0.angularA, bodyParameters.angularFriction.angularA, massA.inverseInertia, + bodyParameters.friction0.angularB, bodyParameters.angularFriction.angularB, massB.inverseInertia), + CalculateInvEffectiveMassOffDiag(bodyParameters.friction1.angularA, bodyParameters.angularFriction.angularA, massA.inverseInertia, + bodyParameters.friction1.angularB, bodyParameters.angularFriction.angularB, massB.inverseInertia)); + + // Invert the matrix and store it to the jacobians + if (!InvertSymmetricMatrix(invEffectiveMassDiag, invEffectiveMassOffDiag, out float3 effectiveMassDiag, out float3 effectiveMassOffDiag)) + { + // invEffectiveMass can be singular if the bodies have infinite inertia about the normal. + // In that case angular friction does nothing so we can regularize the matrix, set col2 = row2 = (0, 0, 1) + invEffectiveMassOffDiag.y = 0.0f; + invEffectiveMassOffDiag.z = 0.0f; + invEffectiveMassDiag.z = 1.0f; + bool success = InvertSymmetricMatrix(invEffectiveMassDiag, + invEffectiveMassOffDiag, + out effectiveMassDiag, + out effectiveMassOffDiag); + Unity.Assertions.Assert.IsTrue(success); // it should never fail, if it does then friction will be disabled + } + bodyParameters.friction0.effectiveMass = effectiveMassDiag.x; + bodyParameters.friction1.effectiveMass = effectiveMassDiag.y; + bodyParameters.angularFriction.effectiveMass = effectiveMassDiag.z; + bodyParameters.frictionEffectiveMassOffDiag = effectiveMassOffDiag; + } + + // Reduce friction to 1/4 of the impulse if there will be restitution + if (applyRestitution) + { + bodyParameters.friction0.effectiveMass *= 0.25f; + bodyParameters.friction1.effectiveMass *= 0.25f; + bodyParameters.angularFriction.effectiveMass *= 0.25f; + bodyParameters.frictionEffectiveMassOffDiag *= 0.25f; + } + } + } + + // Returns true if a collision event was detected. + // perContactImpulses should be initialized to 0f prior to the first solver iteration call (unless you know what you are doing). + /// + /// Performs a solver iteration between the two potentially contacting bodies, updating velocity values and computing impulses. + /// + /// The velocity of the first body. For a static body, this can be stack-allocated to a default value and discarded afterwards. + /// The mass of the first body. Can be default for a static body. + /// Stabilization state for the first body. Can be set to kDefault when not using stabilization. + /// The velocity of the second body. For a static body, this can be stack-allocated to a default value and discarded afterwards. + /// The mass of the second body. Can be default for a static body. + /// Stabilization state for the first body. Can be set to kDefault when not using stabilization. + /// Parameters per contact that describe how each speculative contact point may approach actual contact relative to velocity + /// Accummulated impulses per contact from each solver iteration. These should be initialized to 0f prior to the first solver iteration. + /// Parameters between the bodies that describes friction characteristics and the contact plane. + /// If true, enables the friction velocities heuristic from the motion stabilization inputs. + /// The reciprocal of the number of solver iterations. That is, for 4 solver iterations, this would be 0.25f + /// Impulses computed by this solver iteration + /// True if a collision was detected during this solve operation, meaning that a collision occurred within the timestep + public static bool SolveJacobian(ref Velocity velocityA, in Mass massA, in MotionStabilizationInput motionStabilizationSolverInputA, + ref Velocity velocityB, in Mass massB, in MotionStabilizationInput motionStabilizationSolverInputB, + ReadOnlySpan perContactParameters, Span perContactImpulses, + in ContactJacobianBodyParameters bodyParameters, + bool enableFrictionVelocitiesHeuristic, float invNumSolverIterations, + out ContactJacobianImpulses outputImpulses) + { + // Copy velocity data + Velocity tempVelocityA = velocityA; + Velocity tempVelocityB = velocityB; + + // Solve normal impulses + bool hasCollisionEvent = false; + float sumImpulses = 0.0f; + outputImpulses = default; + + for (int j = 0; j < perContactParameters.Length; j++) + { + ref readonly ContactJacobianContactParameters jacAngular = ref perContactParameters[j]; + var contactImpulse = perContactImpulses[j]; + + // Solve velocity so that predicted contact distance is greater than or equal to zero + float relativeVelocity = GetJacVelocity(bodyParameters.contactNormal, jacAngular.jacobianAngular, + tempVelocityA.linear, tempVelocityA.angular, tempVelocityB.linear, tempVelocityB.angular); + float dv = jacAngular.velocityToReachContactPlane - relativeVelocity; + + float impulse = dv * jacAngular.jacobianAngular.effectiveMass; + float accumulatedImpulse = math.max(contactImpulse + impulse, 0.0f); + if (accumulatedImpulse != contactImpulse) + { + float deltaImpulse = accumulatedImpulse - contactImpulse; + ApplyImpulse(deltaImpulse, bodyParameters.contactNormal, jacAngular.jacobianAngular, ref tempVelocityA, ref tempVelocityB, in massA, in massB, + motionStabilizationSolverInputA.inverseInertiaScale, motionStabilizationSolverInputB.inverseInertiaScale); + } + + contactImpulse = accumulatedImpulse; + perContactImpulses[j] = contactImpulse; + sumImpulses += accumulatedImpulse; + outputImpulses.combinedContactPointsImpulse += contactImpulse; + + // Force contact event even when no impulse is applied, but there is penetration. + hasCollisionEvent |= jacAngular.velocityToReachContactPlane > 0.0f; + } + + // Export collision event + hasCollisionEvent |= outputImpulses.combinedContactPointsImpulse > 0.0f; + + // Solve friction + if (sumImpulses > 0.0f) + { + // Choose friction axes + mathex.GetDualPerpendicularNormalized(bodyParameters.contactNormal, out float3 frictionDir0, out float3 frictionDir1); + + // Calculate impulses for full stop + float3 imp; + { + // Take velocities that produce minimum energy (between input and solver velocity) as friction input + float3 frictionLinVelA = tempVelocityA.linear; + float3 frictionAngVelA = tempVelocityA.angular; + float3 frictionLinVelB = tempVelocityB.linear; + float3 frictionAngVelB = tempVelocityB.angular; + if (enableFrictionVelocitiesHeuristic) + { + GetFrictionVelocities(motionStabilizationSolverInputA.inputVelocity.linear, motionStabilizationSolverInputA.inputVelocity.angular, + tempVelocityA.linear, tempVelocityA.angular, + math.rcp(massA.inverseInertia), math.rcp(massA.inverseMass), + out frictionLinVelA, out frictionAngVelA); + GetFrictionVelocities(motionStabilizationSolverInputB.inputVelocity.linear, motionStabilizationSolverInputB.inputVelocity.angular, + tempVelocityB.linear, tempVelocityB.angular, + math.rcp(massB.inverseInertia), math.rcp(massB.inverseMass), + out frictionLinVelB, out frictionAngVelB); + } + + // Calculate the jacobian dot velocity for each of the friction jacobians + float dv0 = bodyParameters.surfaceVelocityDv.x - GetJacVelocity(frictionDir0, + bodyParameters.friction0, + frictionLinVelA, + frictionAngVelA, + frictionLinVelB, + frictionAngVelB); + float dv1 = bodyParameters.surfaceVelocityDv.y - GetJacVelocity(frictionDir1, + bodyParameters.friction1, + frictionLinVelA, + frictionAngVelA, + frictionLinVelB, + frictionAngVelB); + float dva = bodyParameters.surfaceVelocityDv.z - math.csum( + bodyParameters.angularFriction.angularA * frictionAngVelA + bodyParameters.angularFriction.angularB * frictionAngVelB); + + // Reassemble the effective mass matrix + float3 effectiveMassDiag = new float3(bodyParameters.friction0.effectiveMass, + bodyParameters.friction1.effectiveMass, + bodyParameters.angularFriction.effectiveMass); + float3x3 effectiveMass = BuildSymmetricMatrix(effectiveMassDiag, bodyParameters.frictionEffectiveMassOffDiag); + + // Calculate the impulse + imp = math.mul(effectiveMass, new float3(dv0, dv1, dva)); + } + + // Clip TODO.ma calculate some contact radius and use it to influence balance between linear and angular friction + float maxImpulse = sumImpulses * bodyParameters.coefficientOfFriction * invNumSolverIterations; + float frictionImpulseSquared = math.lengthsq(imp); + imp *= math.min(1.0f, maxImpulse * math.rsqrt(frictionImpulseSquared)); + + // Apply impulses + ApplyImpulse(imp.x, frictionDir0, bodyParameters.friction0, ref tempVelocityA, ref tempVelocityB, + in massA, in massB, + motionStabilizationSolverInputA.inverseInertiaScale, motionStabilizationSolverInputB.inverseInertiaScale); + ApplyImpulse(imp.y, frictionDir1, bodyParameters.friction1, ref tempVelocityA, ref tempVelocityB, + in massA, in massB, + motionStabilizationSolverInputA.inverseInertiaScale, motionStabilizationSolverInputB.inverseInertiaScale); + + tempVelocityA.angular += imp.z * bodyParameters.angularFriction.angularA * motionStabilizationSolverInputA.inverseInertiaScale * massA.inverseInertia; + tempVelocityB.angular += imp.z * bodyParameters.angularFriction.angularB * motionStabilizationSolverInputB.inverseInertiaScale * massB.inverseInertia; + + // Accumulate them + outputImpulses.friction0Impulse = imp.x; + outputImpulses.friction1Impulse = imp.y; + outputImpulses.frictionAngularImpulse = imp.z; + } + + // Write back linear and angular velocities. Changes to other properties, like InverseMass, should not be persisted. + velocityA = tempVelocityA; + velocityB = tempVelocityB; + + return hasCollisionEvent; + } + + static void BuildJacobianAngular(quaternion inverseRotationA, quaternion inverseRotationB, float3 normal, float3 armA, float3 armB, + float3 invInertiaA, float3 invInertiaB, float sumInvMass, out float3 angularA, out float3 angularB, out float invEffectiveMass) + { + float3 crossA = math.cross(armA, normal); + angularA = math.mul(inverseRotationA, crossA).xyz; + + float3 crossB = math.cross(normal, armB); + angularB = math.mul(inverseRotationB, crossB).xyz; + + float3 temp = angularA * angularA * invInertiaA + angularB * angularB * invInertiaB; + invEffectiveMass = temp.x + temp.y + temp.z + sumInvMass; + } + + static float GetJacVelocity(float3 linear, ContactJacobianAngular jacAngular, + float3 linVelA, float3 angVelA, float3 linVelB, float3 angVelB) + { + float3 temp = (linVelA - linVelB) * linear; + temp += angVelA * jacAngular.angularA; + temp += angVelB * jacAngular.angularB; + return math.csum(temp); + } + + private static void ApplyImpulse( + float impulse, float3 linear, ContactJacobianAngular jacAngular, + ref Velocity velocityA, ref Velocity velocityB, + in Mass massA, in Mass massB, + float inverseInertiaScaleA = 1.0f, float inverseInertiaScaleB = 1.0f) + { + velocityA.linear += impulse * linear * massA.inverseMass; + velocityB.linear -= impulse * linear * massB.inverseMass; + + // Scale the impulse with inverseInertiaScale + velocityA.angular += impulse * jacAngular.angularA * inverseInertiaScaleA * massA.inverseInertia; + velocityB.angular += impulse * jacAngular.angularB * inverseInertiaScaleB * massB.inverseInertia; + } + + static void GetFrictionVelocities( + float3 inputLinearVelocity, float3 inputAngularVelocity, + float3 intermediateLinearVelocity, float3 intermediateAngularVelocity, + float3 inertia, float mass, + out float3 frictionLinearVelocityOut, out float3 frictionAngularVelocityOut) + { + float inputEnergy; + { + float linearEnergySq = mass * math.lengthsq(inputLinearVelocity); + float angularEnergySq = math.dot(inertia * inputAngularVelocity, inputAngularVelocity); + inputEnergy = linearEnergySq + angularEnergySq; + } + + float intermediateEnergy; + { + float linearEnergySq = mass * math.lengthsq(intermediateLinearVelocity); + float angularEnergySq = math.dot(inertia * intermediateAngularVelocity, intermediateAngularVelocity); + intermediateEnergy = linearEnergySq + angularEnergySq; + } + + if (inputEnergy < intermediateEnergy) + { + // Make sure we don't change the sign of intermediate velocity when using the input one. + // If sign was to be changed, zero it out since it produces less energy. + bool3 changedSignLin = inputLinearVelocity * intermediateLinearVelocity < float3.zero; + bool3 changedSignAng = inputAngularVelocity * intermediateAngularVelocity < float3.zero; + frictionLinearVelocityOut = math.select(inputLinearVelocity, float3.zero, changedSignLin); + frictionAngularVelocityOut = math.select(inputAngularVelocity, float3.zero, changedSignAng); + } + else + { + frictionLinearVelocityOut = intermediateLinearVelocity; + frictionAngularVelocityOut = intermediateAngularVelocity; + } + } + + // Calculate the inverse effective mass for a pair of jacobians with perpendicular linear parts + static float CalculateInvEffectiveMassOffDiag(float3 angA0, float3 angA1, float3 invInertiaA, + float3 angB0, float3 angB1, float3 invInertiaB) + { + return math.csum(angA0 * angA1 * invInertiaA + angB0 * angB1 * invInertiaB); + } + + // Inverts a symmetric 3x3 matrix with diag = (0, 0), (1, 1), (2, 2), offDiag = (0, 1), (0, 2), (1, 2) = (1, 0), (2, 0), (2, 1) + static bool InvertSymmetricMatrix(float3 diag, float3 offDiag, out float3 invDiag, out float3 invOffDiag) + { + float3 offDiagSq = offDiag.zyx * offDiag.zyx; + float determinant = (mathex.cproduct(diag) + 2.0f * mathex.cproduct(offDiag) - math.csum(offDiagSq * diag)); + bool determinantOk = (determinant != 0); + float invDeterminant = math.select(0.0f, 1.0f / determinant, determinantOk); + invDiag = (diag.yxx * diag.zzy - offDiagSq) * invDeterminant; + invOffDiag = (offDiag.yxx * offDiag.zzy - diag.zyx * offDiag) * invDeterminant; + return determinantOk; + } + + // Builds a symmetric 3x3 matrix from diag = (0, 0), (1, 1), (2, 2), offDiag = (0, 1), (0, 2), (1, 2) = (1, 0), (2, 0), (2, 1) + static float3x3 BuildSymmetricMatrix(float3 diag, float3 offDiag) + { + return new float3x3( + new float3(diag.x, offDiag.x, offDiag.y), + new float3(offDiag.x, diag.y, offDiag.z), + new float3(offDiag.y, offDiag.z, diag.z) + ); + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + static void CheckContactAndJacobianSpanLengthsEqual(int parametersLength, int contactsLength) + { + if (parametersLength != contactsLength) + throw new ArgumentException($"Span length of {parametersLength} does not match the number of contacts {contactsLength}"); + } + } +} + diff --git a/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContactJacobians.cs.meta b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContactJacobians.cs.meta new file mode 100644 index 0000000..cbbde2f --- /dev/null +++ b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContactJacobians.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9726f304892316c4190af0f1cb070dc1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContacts.cs b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContacts.cs index 7552a96..7dfa33c 100644 --- a/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContacts.cs +++ b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimContacts.cs @@ -1,3 +1,4 @@ +using System; using System.Diagnostics; using Latios.Transforms; using Unity.Collections; @@ -57,10 +58,24 @@ public static ContactsBetweenResult ContactsBetween(in Collider colliderA, /// public unsafe struct ContactsBetweenResult { - public float3 contactNormal; - public int contactCount; + /// + /// A vector that points from a non-penetrating contact on collider "B" + /// to the surface on collider "A". The normal should always be outward + /// from "B". + /// + public float3 contactNormal; + /// + /// The number of contacts contained + /// + public int contactCount; + /// + /// The raw memory storage of the contacts + /// public fixed float contactsData[128]; + /// + /// Retrieves the contact at the specified index + /// public ref ContactOnB this[int index] { get @@ -71,6 +86,9 @@ public ref ContactOnB this[int index] } } + /// + /// Adds a new contact point. The safety check will fail if there are already 32 contacts. + /// public void Add(ContactOnB contact) { CheckCapacityBeforeAdd(); @@ -79,11 +97,18 @@ public void Add(ContactOnB contact) this[index] = contact; } + /// + /// Adds a new contact point. The safety check will fail if there are already 32 contacts. + /// public void Add(float3 locationOnB, float distanceToA) { Add(new ContactOnB { location = locationOnB, distanceToA = distanceToA }); } + /// + /// Removes the contact at the specified index, and the contact at the highest index will + /// take its place (as long as that is not the contact being removed). + /// public void RemoveAtSwapBack(int index) { CheckInRange(index); @@ -91,6 +116,10 @@ public void RemoveAtSwapBack(int index) contactCount--; } + /// + /// Flips the perspective of the contacts such that they lie on "A" + /// with the contact normal pointed in the opposite direction. + /// public void FlipInPlace() { for (int i = 0; i < contactCount; i++) @@ -102,6 +131,10 @@ public void FlipInPlace() contactNormal = -contactNormal; } + /// + /// Creates a flipped perspective of the contacts such that they lie on "A" + /// with the contact normal pointed in the opposite direction. + /// public ContactsBetweenResult ToFlipped() { var result = this; @@ -109,14 +142,38 @@ public ContactsBetweenResult ToFlipped() return result; } + /// + /// Acquires a span of the contacts. The Span MUST only be used while this + /// ContactsBetweenResult instance is valid. + /// + /// + public Span AsSpan() + { + fixed (ContactOnB* ptr = &this[0]) + return new Span(ptr, contactCount); + } + + /// + /// A contact that lies on the "B" collider in a pair of colliders + /// public struct ContactOnB { + /// + /// A packed representation of the contact + /// public float4 contactData; + /// + /// The position of the contact on the surface of the "B" collider + /// public float3 location { get => contactData.xyz; set => contactData.xyz = value; } + /// + /// The distance of the contact to the surface of the "A" collider + /// along the contact normal. Negative if the contact is penetrating. + /// public float distanceToA { get => contactData.w; diff --git a/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimMotion.cs b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimMotion.cs new file mode 100644 index 0000000..77e17d3 --- /dev/null +++ b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimMotion.cs @@ -0,0 +1,166 @@ +using Latios.Transforms; +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; + +namespace Latios.Psyshock +{ + public static partial class UnitySim + { + /// + /// A velocity comprised of linear and angular components. + /// The linear component is in world-space while the angular + /// ccomponent is relative to the local inertia tensor diagonal + /// + public struct Velocity + { + public float3 linear; + public float3 angular; + } + + /// + /// Represents inverse mass and inertia used in solvers. + /// Zero values make the object immovable for the corresponding + /// component. + /// + public struct Mass + { + public float inverseMass; + public float3 inverseInertia; + } + + /// + /// A struct used to define collision search ranges for an object in motion + /// + public struct MotionExpansion + { + float4 uniformXlinearYzw; + + /// + /// Creates a motion expansion given the velocity, timestep, and angular expansion factor of the object. + /// Forces such as gravity should be applied to the velocity prior to calling this. + /// + /// The velocity of the object, after forces have been applied. + /// + /// + public MotionExpansion(in Velocity velocity, float deltaTime, float angularExpansionFactor) + { + var linear = velocity.linear * deltaTime; + // math.length(AngularVelocity) * timeStep is conservative approximation of sin((math.length(AngularVelocity) * timeStep) + var uniform = 0.05f + math.min(math.length(velocity.angular) * deltaTime * angularExpansionFactor, angularExpansionFactor); + uniformXlinearYzw = new float4(uniform, linear); + } + + /// + /// Expands a collider's world-space Aabb obtained via AabbFrom to account for its motion + /// + /// The Aabb typically obtained from calling AabbFrom on a Collider + /// An expanded version of the Collider's Aabb accounting for its anticipated motion + /// within a timestep. + public Aabb ExpandAabb(Aabb aabb) + { + var linear = uniformXlinearYzw.yzw; + aabb.min = math.min(aabb.min, aabb.min + linear) - uniformXlinearYzw.x; + aabb.max = math.max(aabb.max, aabb.max + linear) + uniformXlinearYzw.x; + return aabb; + } + + /// + /// Gets the max search distance between the moving collider and a static object. + /// The search distance should be used in a call to DistanceBetween() or DistanceBetweenAll(). + /// + /// The motion expansion of the moving collider + /// The max search distance required to anticipate contact within a timestep + public static float GetMaxDistance(in MotionExpansion motionExpansion) + { + return math.length(motionExpansion.uniformXlinearYzw.yzw) + motionExpansion.uniformXlinearYzw.x + 0.05f; + } + + /// + /// Gets the max search distance between two moving colliders. + /// The search distance should be used in a call to DistanceBetween() or DistanceBetweenAll(). + /// + /// The motion expansion of the first moving collider + /// The motion expansion of the second moving collider + /// The max search distance required to anticipate contact within a timestep + public static float GetMaxDistance(in MotionExpansion motionExpansionA, in MotionExpansion motionExpansionB) + { + var tempB = motionExpansionB.uniformXlinearYzw; + tempB.x = -tempB.x; + var tempCombined = motionExpansionA.uniformXlinearYzw - tempB; + return math.length(tempCombined.yzw) + tempCombined.x; + } + } + + /// + /// Updates a world transform of an object and its velocity using the specified time step and damping + /// + /// The world transform of the center of mass and inertia tensor diagonal + /// The linear and angular velocity to apply to the world transform while also being damped + /// The amount of linear damping to apply per second, typically a small value near 0 + /// The amount of angular damping to apply per second, typically a small value near 0 + /// The time step over which the values should be updated + public static void Integrate(ref RigidTransform inertialPoseWorldTransform, ref Velocity velocity, float linearDamping, float angularDamping, float deltaTime) + { + inertialPoseWorldTransform.pos += velocity.linear * deltaTime; + var halfDeltaAngle = velocity.angular * 0.5f * deltaTime; + var dq = new quaternion(new float4(halfDeltaAngle, 1f)); + inertialPoseWorldTransform.rot = math.normalize(math.mul(inertialPoseWorldTransform.rot, dq)); + + var dampFactors = math.clamp(1f - new float2(linearDamping, angularDamping) * deltaTime, 0f, 1f); + velocity.linear *= dampFactors.x; + velocity.angular *= dampFactors.y; + } + + /// + /// Applies the transform delta relative to the center of mass and inertia tensor diagonal to a TransformQvvs + /// + /// The previous world transform for an entity + /// The previous world transform of the entity's center of mass and inertia tensor diagonal + /// The new world transform of the entity's center of mass and inertia tensor diagonal + /// A new world transform of the entity based on the delta of the center of mass and inertia tensor diagonal + public static TransformQvvs ApplyInertialPoseWorldTransformDeltaToWorldTransform(in TransformQvvs oldWorldTransform, + in RigidTransform oldInertialPoseWorldTransform, + in RigidTransform newInertialPoseWorldTransform) + { + var oldTransform = new RigidTransform(oldWorldTransform.rotation, oldWorldTransform.position); + // oldInertialPoseWorldTransform = oldWorldTransform * localInertial + // newInertialPoseWorldTransfrom = newWorldTransform * localInertial + // inverseOldWorldTransform * oldInertialWorldTransform = inverseOldWorldTransform * oldWorldTransform * localInertial + // inverseOldWorldTransform * oldInertialWorldTransform = localInertial + // newInertialPoseWorldTransform * inverseLocalInertial = newWorldTransform * localInertial * inverseLocalInertial + // newInertialPoseWorldTransform * inverseLocalInertial = newWorldTransform + // newInertialPoseWorldTransform * inverse(inverseOldWorldTransform * oldInertialWorldTransform) = newWorldTransform + // newInertialPoseWorldTransform * inverseOldInertialWorldTransform * oldWorldTransform = newWorldTransform + var newTransform = math.mul(newInertialPoseWorldTransform, math.mul(math.inverse(oldInertialPoseWorldTransform), oldTransform)); + return new TransformQvvs + { + position = newTransform.pos, + rotation = newTransform.rot, + scale = oldWorldTransform.scale, + stretch = oldWorldTransform.stretch, + worldIndex = oldWorldTransform.worldIndex + }; + } + + /// + /// Computes a new world transform for an entity based on the local and world transforms of the inertia tensor diagonal + /// and center of mass + /// + /// The original world transform (only scale, stretch, and worldIndex are used) + /// The world space transform of the center of mass and inertia tensor diagonal + /// The local-space inertia tensor diagonal orientation relative to the entity + /// The local-space center of mass relative to the entity + /// A new world transform of the entity that preserves the world-space center of mass and inertia tensor diagonal + public static TransformQvvs ApplyWorldTransformFromInertialPoses(in TransformQvvs oldWorldTransform, + in RigidTransform inertialPoseWorldTransform, + quaternion localTensorOrientation, + float3 localCenterOfMassUnscaled) + { + var localInertial = new RigidTransform(localTensorOrientation, localCenterOfMassUnscaled * oldWorldTransform.stretch * oldWorldTransform.scale); + var newWorldTransform = math.mul(inertialPoseWorldTransform, math.inverse(localInertial)); + return new TransformQvvs(newWorldTransform.pos, newWorldTransform.rot, oldWorldTransform.scale, oldWorldTransform.stretch, oldWorldTransform.worldIndex); + } + } +} + diff --git a/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimMotion.cs.meta b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimMotion.cs.meta new file mode 100644 index 0000000..962f4ae --- /dev/null +++ b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimMotion.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 70714a158893c4a428011c7408760b73 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimShapeMassUtilities.cs b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimShapeMassUtilities.cs new file mode 100644 index 0000000..61b2930 --- /dev/null +++ b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimShapeMassUtilities.cs @@ -0,0 +1,292 @@ +using Latios.Transforms; +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; + +namespace Latios.Psyshock +{ + public static partial class UnitySim + { + /// + /// A local-space inertia tensor diagonal matrix and orientation. + /// The diagonal should have any stretch already applied to the collider + /// it was derived from. The diagonal is used to compute the inverse inertia + /// in a Mass instance while the orientation is transformed into the + /// inertialPoseWorldTransform. The inertia tensor diagonal is normalized to + /// be independent of the object's mass. + /// + public struct LocalInertiaTensorDiagonal + { + public quaternion tensorOrientation; + public float3 inertiaDiagonal; + + /// + /// Converts the inertia diagonal and orientation back into a singular matrix. + /// This can be useful when combining inertia tensors. + /// + public float3x3 ToMatrix() + { + var r = new float3x3(tensorOrientation); + var r2 = new float3x3(inertiaDiagonal.x * r.c0, inertiaDiagonal.y * r.c1, inertiaDiagonal.z * r.c2); + return math.mul(r2, math.transpose(r)); + } + } + + /// + /// Computes the Mass properties and inertialPoseWorldTransform from the results + /// of calls to LocalCenterOfMassFrom() and LocalInertiaTensorFrom() transformed + /// by the worldTransform. + /// + /// The world transform of the body entity + /// The local inertia tensor diagonal, already stretched by the worldTransform stretch value + /// The center of mass relative to the body entity in the body entity's local space + /// The reciprocal of the mass, where 0 makes the object immovable + /// The Mass containing the inverse mass and inverse inertia + /// A world transform centered around the body's center of mass and oriented relative to the inertia tensor diagonal + public static void ConvertToWorldMassInertia(in TransformQvvs worldTransform, + in LocalInertiaTensorDiagonal localInertiaTensorDiagonal, + float3 localCenterOfMassUnscaled, + float inverseMass, + out Mass massOut, + out RigidTransform inertialPoseWorldTransform) + { + inertialPoseWorldTransform = new RigidTransform(qvvs.TransformRotation(in worldTransform, localInertiaTensorDiagonal.tensorOrientation), + qvvs.TransformPoint(in worldTransform, localCenterOfMassUnscaled)); + massOut = new Mass + { + inverseMass = inverseMass, + inverseInertia = math.rcp(localInertiaTensorDiagonal.inertiaDiagonal) * inverseMass / (worldTransform.scale * worldTransform.scale) + }; + } + + /// + /// Gets the default center of mass computed for the type of collider, in the collider's local space + /// + public static float3 LocalCenterOfMassFrom(in Collider collider) + { + switch (collider.type) + { + case ColliderType.Sphere: + return collider.m_sphere.center; + case ColliderType.Capsule: + return (collider.m_capsule.pointA + collider.m_capsule.pointB) / 2f; + case ColliderType.Box: + return collider.m_box.center; + case ColliderType.Triangle: + return (collider.m_triangle.pointA + collider.m_triangle.pointB + collider.m_triangle.pointC) / 3f; + case ColliderType.Convex: + return collider.m_convex.scale * collider.m_convex.convexColliderBlob.Value.centerOfMass; + case ColliderType.TriMesh: + return (collider.m_triMesh().triMeshColliderBlob.Value.localAabb.min + collider.m_triMesh().triMeshColliderBlob.Value.localAabb.max) / 2f; + case ColliderType.Compound: + return collider.m_compound().scale * collider.m_compound().compoundColliderBlob.Value.centerOfMass; + default: return default; + } + } + + /// + /// Gets the default local center-of-mass-relative-space inertia tensor diagonal from the collider, + /// after stretching the collider. This method can be somewhat expensive, especially for blob-based + /// collider types when scaled, though this cost is independent of the blob size. + /// + /// The collider to compute the local inertia tensor from + /// How much the collider should be stretched by. Use 1f for no stretching. + /// A local space inertia tensor matrix diagonal and orientation + public static LocalInertiaTensorDiagonal LocalInertiaTensorFrom(in Collider collider, float3 stretch) + { + if (stretch.Equals(new float3(1f))) + return LocalInertiaTensorFrom(in collider); + var scaled = Physics.ScaleStretchCollider(collider, 1f, stretch); + return LocalInertiaTensorFrom(in scaled); + } + + /// + /// Gets the default angular expansion factor of the collider, used in MotionExpansion + /// + public static float AngularExpansionFactorFrom(in Collider collider) + { + switch (collider.type) + { + case ColliderType.Sphere: + return 0f; + case ColliderType.Capsule: + return math.distance(collider.m_capsule.pointA, collider.m_capsule.pointB) / 2f; + case ColliderType.Box: + return math.length(collider.m_box.halfSize); + case ColliderType.Triangle: + { + var center = (collider.m_triangle.pointA + collider.m_triangle.pointB + collider.m_triangle.pointC) / 3f; + var distanceSq = math.cmax(simd.distancesq(collider.m_triangle.AsSimdFloat3(), center)); + return math.sqrt(distanceSq); + } + case ColliderType.Convex: + { + ref var blob = ref collider.m_convex.convexColliderBlob.Value; + var aabb = blob.localAabb; + aabb.min *= collider.m_convex.scale; + aabb.max *= collider.m_convex.scale; + var centerOfMass = blob.centerOfMass; + Physics.GetCenterExtents(aabb, out var aabbCenter, out var aabbExtents); + var delta = centerOfMass - aabbCenter; + var extremePoint = math.select(aabbExtents, -aabbExtents, delta >= 0f); + return math.distance(extremePoint, delta); + } + case ColliderType.TriMesh: + { + var aabb = collider.m_triMesh().triMeshColliderBlob.Value.localAabb; + aabb.min *= collider.m_triMesh().scale; + aabb.max *= collider.m_triMesh().scale; + Physics.GetCenterExtents(aabb, out _, out var extents); + return math.length(extents); + } + case ColliderType.Compound: + { + ref var blob = ref collider.m_compound().compoundColliderBlob.Value; + var aabb = blob.localAabb; + aabb.min *= collider.m_compound().scale; + aabb.max *= collider.m_compound().scale; + var centerOfMass = blob.centerOfMass; + Physics.GetCenterExtents(aabb, out var aabbCenter, out var aabbExtents); + var delta = centerOfMass - aabbCenter; + var extremePoint = math.select(aabbExtents, -aabbExtents, delta >= 0f); + return math.distance(extremePoint, delta); + } + default: return default; + } + } + + static LocalInertiaTensorDiagonal LocalInertiaTensorFrom(in Collider collider) + { + switch (collider.type) + { + case ColliderType.Sphere: + return new LocalInertiaTensorDiagonal + { + inertiaDiagonal = (2f / 5f) * collider.m_sphere.radius * collider.m_sphere.radius, + tensorOrientation = quaternion.identity + }; + case ColliderType.Capsule: + { + float3 axis = collider.m_capsule.pointB - collider.m_capsule.pointA; + float lengthSq = math.lengthsq(axis); + float length = math.sqrt(lengthSq); + if (lengthSq == 0f || !math.isfinite(length)) + { + new LocalInertiaTensorDiagonal + { + inertiaDiagonal = (2f / 5f) * collider.m_capsule.radius * collider.m_capsule.radius, + tensorOrientation = quaternion.identity + }; + } + + float radius = collider.m_capsule.radius; + float radiusSq = radius * radius; + + float cylinderMassPart = math.PI * length * radiusSq; + float sphereMassPart = math.PI * (4f / 3f) * radiusSq * radius; + float totalMass = cylinderMassPart + sphereMassPart; + cylinderMassPart /= totalMass; + sphereMassPart /= totalMass; + + float onAxisInertia = (cylinderMassPart / 2f + sphereMassPart * 2f / 5f) * radiusSq; + float offAxisInertia = cylinderMassPart * (radiusSq / 4f + lengthSq / 12f) + + sphereMassPart * (radiusSq * 2f / 5f + radius * length * 3f / 8f + lengthSq / 4f); + + return new LocalInertiaTensorDiagonal + { + inertiaDiagonal = new float3(offAxisInertia, onAxisInertia, offAxisInertia), + tensorOrientation = mathex.FromToRotation(new float3(0f, 1f, 0f), math.normalize(axis)) + }; + } + case ColliderType.Box: + { + var halfSq = collider.m_box.halfSize * collider.m_box.halfSize; + float3 tensorNum = new float3(halfSq.y + halfSq.z, halfSq.x + halfSq.z, halfSq.x + halfSq.y); + return new LocalInertiaTensorDiagonal + { + inertiaDiagonal = tensorNum / 3f, + tensorOrientation = quaternion.identity + }; + } + case ColliderType.Triangle: + { + var center = (collider.m_triangle.pointA + collider.m_triangle.pointB + collider.m_triangle.pointC) / 3f; + var distanceSq = math.cmax(simd.distancesq(collider.m_triangle.AsSimdFloat3(), center)); + return new LocalInertiaTensorDiagonal + { + inertiaDiagonal = (2f / 5f) * distanceSq, + tensorOrientation = quaternion.identity + }; + } + case ColliderType.Convex: + { + ref var blob = ref collider.m_convex.convexColliderBlob.Value; + var scale = collider.m_convex.scale; + if (scale.Equals(new float3(1f, 1f, 1f))) + { + // Fast path + return new LocalInertiaTensorDiagonal + { + inertiaDiagonal = blob.unscaledInertiaTensorDiagonal, + tensorOrientation = blob.unscaledInertiaTensorOrientation + }; + } + + // Todo: Is there a faster way to do this when we already have the unscaled orientation and diagonal? + var scaledTensor = blob.inertiaTensor; + scaledTensor.c0.x *= scale.x; + scaledTensor.c1.y *= scale.y; + scaledTensor.c2.z *= scale.z; + mathex.DiagonalizeSymmetricApproximation(scaledTensor, out var orientation, out var diagonal); + return new LocalInertiaTensorDiagonal + { + inertiaDiagonal = diagonal, + tensorOrientation = new quaternion(orientation) + }; + } + case ColliderType.TriMesh: + { + var aabb = collider.m_triMesh().triMeshColliderBlob.Value.localAabb; + aabb.min *= collider.m_triMesh().scale; + aabb.max *= collider.m_triMesh().scale; + Physics.GetCenterExtents(aabb, out _, out var extents); + var halfSq = extents * extents; + float3 tensorNum = new float3(halfSq.y + halfSq.z, halfSq.x + halfSq.z, halfSq.x + halfSq.y); + return new LocalInertiaTensorDiagonal + { + inertiaDiagonal = tensorNum / 3f, + tensorOrientation = quaternion.identity + }; + } + case ColliderType.Compound: + { + ref var blob = ref collider.m_compound().compoundColliderBlob.Value; + var scale = collider.m_compound().scale * collider.m_compound().stretch; + if (scale.Equals(new float3(1f, 1f, 1f))) + { + // Fast path + return new LocalInertiaTensorDiagonal + { + inertiaDiagonal = blob.unscaledInertiaTensorDiagonal, + tensorOrientation = blob.unscaledInertiaTensorOrientation + }; + } + + // Todo: Is there a faster way to do this when we already have the unscaled orientation and diagonal? + var scaledTensor = blob.inertiaTensor; + scaledTensor.c0.x *= scale.x; + scaledTensor.c1.y *= scale.y; + scaledTensor.c2.z *= scale.z; + mathex.DiagonalizeSymmetricApproximation(scaledTensor, out var orientation, out var diagonal); + return new LocalInertiaTensorDiagonal + { + inertiaDiagonal = diagonal, + tensorOrientation = new quaternion(orientation) + }; + } + default: return default; + } + } + } +} + diff --git a/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimShapeMassUtilities.cs.meta b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimShapeMassUtilities.cs.meta new file mode 100644 index 0000000..206969f --- /dev/null +++ b/PsyshockPhysics/Physics/Dynamics/UnitySim/UnitySimShapeMassUtilities.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 770f2ff1b749a8e448c60d0a13439012 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/PsyshockPhysics/Physics/Internal/Builders/BuildCollisionLayerInternal.cs b/PsyshockPhysics/Physics/Internal/Builders/BuildCollisionLayerInternal.cs index 553c499..ce523ba 100644 --- a/PsyshockPhysics/Physics/Internal/Builders/BuildCollisionLayerInternal.cs +++ b/PsyshockPhysics/Physics/Internal/Builders/BuildCollisionLayerInternal.cs @@ -1,8 +1,11 @@ -using Latios.Transforms; +using System; +using System.Diagnostics; +using Latios.Transforms; using Unity.Burst; using Unity.Burst.Intrinsics; using Unity.Collections; using Unity.Entities; +using Unity.Entities.UniversalDelegates; using Unity.Jobs; using Unity.Mathematics; @@ -18,11 +21,20 @@ public struct ColliderAoSData public Entity entity; } + public struct FilteredChunkCache + { + public ArchetypeChunk chunk; + public v128 mask; + public int firstEntityIndex; + public int countInChunk; + public bool usesMask; + } + #region Jobs - //Parallel - //Calculate RigidTransform, AABB, and target bucket. Write the targetBucket as the layerIndex + // Parallel + // Calculate Aabb and target bucket. Write the targetBucket as the layerIndex [BurstCompile] - public struct Part1FromQueryJob : IJobChunk + public struct Part1FromUnfilteredQueryJob : IJobChunk { [ReadOnly] public CollisionLayer layer; [NoAlias, NativeDisableParallelForRestriction] public NativeArray layerIndices; @@ -33,7 +45,114 @@ public struct Part1FromQueryJob : IJobChunk public void Execute(in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask) { - var firstEntityIndex = firstEntityInChunkIndices[unfilteredChunkIndex]; + var chunkEntities = chunk.GetNativeArray(typeGroup.entity); + var chunkColliders = chunk.GetNativeArray(ref typeGroup.collider); + var chunkTransforms = typeGroup.worldTransform.Resolve(chunk); + for (int src = 0, dst = firstEntityInChunkIndices[unfilteredChunkIndex]; src < chunk.Count; src++, dst++) + { + var collider = chunkColliders[src]; + var transform = chunkTransforms[src]; + var entity = chunkEntities[src]; + + Aabb aabb = Physics.AabbFrom(in collider, transform.worldTransformQvvs); + + colliderAoS[dst] = new ColliderAoSData + { + collider = collider, + transform = transform.worldTransformQvvs, + aabb = aabb, + entity = entity + }; + xMinMaxs[dst] = new float2(aabb.min.x, aabb.max.x); + + int3 minBucket = math.int3(math.floor((aabb.min - layer.worldMin) / layer.worldAxisStride)); + int3 maxBucket = math.int3(math.floor((aabb.max - layer.worldMin) / layer.worldAxisStride)); + minBucket = math.clamp(minBucket, 0, layer.worldSubdivisionsPerAxis - 1); + maxBucket = math.clamp(maxBucket, 0, layer.worldSubdivisionsPerAxis - 1); + + if (math.any(math.isnan(aabb.min) | math.isnan(aabb.max))) + { + layerIndices[dst] = layer.bucketStartsAndCounts.Length - 1; + } + else if (math.all(minBucket == maxBucket)) + { + layerIndices[dst] = (minBucket.x * layer.worldSubdivisionsPerAxis.y + minBucket.y) * layer.worldSubdivisionsPerAxis.z + minBucket.z; + } + else + { + layerIndices[dst] = layer.bucketStartsAndCounts.Length - 2; + } + } + } + } + + // Single + // Create a cache of chunks, their masks, and their counts to preallocate the arrays without having to process filters twice. + [BurstCompile] + public struct Part0PrefilterQueryJob : IJobChunk + { + public NativeList filteredChunkCache; + int countAccumulated; + + public void Execute(in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask) + { + int filteredCount = math.select(chunk.Count, math.countbits(chunkEnabledMask.ULong0) + math.countbits(chunkEnabledMask.ULong1), useEnabledMask); + filteredChunkCache.AddNoResize(new FilteredChunkCache + { + chunk = chunk, + mask = chunkEnabledMask, + firstEntityIndex = countAccumulated, + countInChunk = filteredCount, + usesMask = useEnabledMask + }); + countAccumulated += filteredCount; + } + } + + // Single + // Allocate the CollisionLayer NativeLists and temp lists to the correct size using the gathers cached chunks. + [BurstCompile] + public struct AllocateCollisionLayerFromFilteredQueryJob : IJob + { + public CollisionLayer layer; + public NativeList layerIndices; + public NativeList xMinMaxs; + public NativeList colliderAoS; + [ReadOnly] public NativeArray filteredChunkCache; + + public void Execute() + { + if (filteredChunkCache.Length == 0) + return; + + var last = filteredChunkCache[filteredChunkCache.Length - 1]; + var count = last.firstEntityIndex + last.countInChunk; + layer.ResizeUninitialized(count); + layerIndices.ResizeUninitialized(count); + xMinMaxs.ResizeUninitialized(count); + colliderAoS.ResizeUninitialized(count); + } + } + + // Parallel + // Calculate Aabb and target bucket. Write the targetBucket as the layerIndex + [BurstCompile] + public struct Part1FromFilteredQueryJob : IJobParallelForDefer + { + [ReadOnly] public CollisionLayer layer; + [NoAlias, NativeDisableParallelForRestriction] public NativeArray layerIndices; + [NoAlias, NativeDisableParallelForRestriction] public NativeArray colliderAoS; + [NoAlias, NativeDisableParallelForRestriction] public NativeArray xMinMaxs; + [ReadOnly] public BuildCollisionLayerTypeHandles typeGroup; + [ReadOnly] public NativeArray filteredChunkCache; + + public void Execute(int chunkIndex) + { + var filteredChunk = filteredChunkCache[chunkIndex]; + var chunk = filteredChunk.chunk; + var firstEntityIndex = filteredChunk.firstEntityIndex; + var useEnabledMask = filteredChunk.usesMask; + var chunkEnabledMask = filteredChunk.mask; var chunkEntities = chunk.GetNativeArray(typeGroup.entity); var chunkColliders = chunk.GetNativeArray(ref typeGroup.collider); @@ -77,12 +196,41 @@ public void Execute(in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useE } } - //Parallel - //Calculated Target Bucket and write as layer index + // Single + // Allocate the CollisionLayer NativeLists and temp lists to the correct size using the bodies list [BurstCompile] - public struct Part1FromColliderBodyArrayJob : IJob, IJobParallelFor + public struct AllocateCollisionLayerFromBodiesListJob : IJob { public CollisionLayer layer; + public NativeList layerIndices; + public NativeList xMinMaxs; + public NativeList aabbs; + [ReadOnly] public NativeArray bodies; + + public bool aabbsAreProvided; + + public void Execute() + { + layer.ResizeUninitialized(bodies.Length); + layerIndices.ResizeUninitialized(bodies.Length); + xMinMaxs.ResizeUninitialized(bodies.Length); + if (aabbsAreProvided) + { + ValidateOverrideAabbsAreRightLength(aabbs.AsArray(), bodies.Length); + } + else + { + aabbs.ResizeUninitialized(bodies.Length); + } + } + } + + // Parallel + // Calculate target bucket and write as layer index + [BurstCompile] + public struct Part1FromColliderBodyArrayJob : IJob, IJobParallelForDefer + { + [ReadOnly] public CollisionLayer layer; [NoAlias] public NativeArray layerIndices; [ReadOnly] public NativeArray colliderBodies; [NoAlias] public NativeArray aabbs; @@ -120,12 +268,12 @@ public void Execute(int i) } } - //Parallel - //Calculated Target Bucket and write as layer index using the override AABB + // Parallel + // Calculate target bucket and write as layer index using the override Aabb [BurstCompile] - public struct Part1FromDualArraysJob : IJob, IJobParallelFor + public struct Part1FromDualArraysJob : IJob, IJobParallelForDefer { - public CollisionLayer layer; + [ReadOnly] public CollisionLayer layer; [NoAlias] public NativeArray layerIndices; [ReadOnly] public NativeArray aabbs; [NoAlias] public NativeArray xMinMaxs; @@ -138,6 +286,8 @@ public void Execute() public void Execute(int i) { + ValidateOverrideAabbsAreRightLength(aabbs, layerIndices.Length); + var aabb = aabbs[i]; xMinMaxs[i] = new float2(aabb.min.x, aabb.max.x); @@ -161,8 +311,8 @@ public void Execute(int i) } } - //Single - //Count total in each bucket and assign global array position to layerIndex + // Single + // Count total in each bucket and assign global array position to layerIndex [BurstCompile] public struct Part2Job : IJob { @@ -194,13 +344,13 @@ public void Execute() } } - //Parallel - //Reverse array of dst indices to array of src indices - //Todo: Might be faster as an IJob due to potential false sharing + // Parallel + // Reverse array of dst indices to array of src indices + // Todo: Might be faster as an IJob due to potential false sharing [BurstCompile] - public struct Part3Job : IJob, IJobParallelFor + public struct Part3Job : IJob, IJobParallelForDefer { - [ReadOnly, DeallocateOnJobCompletion] public NativeArray layerIndices; + [ReadOnly] public NativeArray layerIndices; [NoAlias, NativeDisableParallelForRestriction] public NativeArray unsortedSrcIndices; public void Execute() @@ -216,14 +366,14 @@ public void Execute(int i) } } - //Parallel - //Sort buckets + // Parallel + // Sort buckets and build interval trees [BurstCompile] public struct Part4Job : IJob, IJobParallelFor { [NoAlias, NativeDisableParallelForRestriction] public NativeArray unsortedSrcIndices; [NoAlias, NativeDisableParallelForRestriction] public NativeArray trees; - [ReadOnly, DeallocateOnJobCompletion] public NativeArray xMinMaxs; + [ReadOnly] public NativeArray xMinMaxs; [ReadOnly] public NativeArray bucketStartAndCounts; public void Execute() @@ -261,15 +411,15 @@ public void Execute(int i) } }*/ - //Parallel - //Copy AoS data to SoA layer + // Parallel + // Copy AoS data to SoA layer [BurstCompile] - public struct Part5FromQueryJob : IJob, IJobParallelFor + public struct Part5FromAoSJob : IJob, IJobParallelForDefer { [NoAlias, NativeDisableParallelForRestriction] public CollisionLayer layer; - [ReadOnly, DeallocateOnJobCompletion] + [ReadOnly] public NativeArray colliderAoS; public void Execute() @@ -293,10 +443,10 @@ public void Execute(int i) } } - //Parallel - //Copy array data to layer + // Parallel + // Copy array data to layer [BurstCompile] - public struct Part5FromArraysJob : IJob, IJobParallelFor + public struct Part5FromSplitArraysJob : IJob, IJobParallelForDefer { [NativeDisableParallelForRestriction] public CollisionLayer layer; @@ -320,8 +470,23 @@ public void Execute(int i) } } - //Single - //All five steps for custom arrays + // Single + // All five steps plus allocation for chunk caches + [BurstCompile] + public struct BuildFromFilteredChunkCacheSingleJob : IJob + { + public CollisionLayer layer; + [ReadOnly] public NativeArray filteredChunkCache; + public BuildCollisionLayerTypeHandles handles; + + public void Execute() + { + BuildImmediate(ref layer, filteredChunkCache, in handles); + } + } + + // Single + // All five steps for custom bodies array [BurstCompile] public struct BuildFromColliderArraySingleJob : IJob { @@ -335,8 +500,8 @@ public void Execute() } } - //Single - //All five steps for custom arrays + // Single + // All five steps for custom arrays [BurstCompile] public struct BuildFromDualArraysSingleJob : IJob { @@ -354,12 +519,45 @@ public void Execute() #endregion #region Immediate + public static void BuildImmediate(ref CollisionLayer layer, NativeArray filteredChunkCache, in BuildCollisionLayerTypeHandles handles) + { + int count = 0; + if (filteredChunkCache.Length != 0) + { + var last = filteredChunkCache[filteredChunkCache.Length - 1]; + count = last.firstEntityIndex + last.countInChunk; + } + layer.ResizeUninitialized(count); + + var layerIndices = new NativeArray(count, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var xMinMaxs = new NativeArray(count, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var colliderAoS = new NativeArray(count, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + var p1 = new Part1FromFilteredQueryJob + { + colliderAoS = colliderAoS, + layer = layer, + layerIndices = layerIndices, + xMinMaxs = xMinMaxs, + filteredChunkCache = filteredChunkCache, + typeGroup = handles + }; + for (int i = 0; i < layer.count; i++) + { + p1.Execute(i); + } + + BuildImmediateAoS2To5(ref layer, colliderAoS, layerIndices, xMinMaxs); + } + public static void BuildImmediate(ref CollisionLayer layer, NativeArray bodies) { var aabbs = new NativeArray(bodies.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); var layerIndices = new NativeArray(bodies.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); var xMinMaxs = new NativeArray(bodies.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + layer.ResizeUninitialized(bodies.Length); + var p1 = new Part1FromColliderBodyArrayJob { aabbs = aabbs, @@ -373,6 +571,38 @@ public static void BuildImmediate(ref CollisionLayer layer, NativeArray bodies, NativeArray aabbs) + { + ValidateOverrideAabbsAreRightLength(aabbs, bodies.Length); + + var layerIndices = new NativeArray(bodies.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var xMinMaxs = new NativeArray(bodies.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + layer.ResizeUninitialized(bodies.Length); + + var p1 = new Part1FromDualArraysJob + { + aabbs = aabbs, + layer = layer, + layerIndices = layerIndices, + xMinMaxs = xMinMaxs + }; + for (int i = 0; i < layer.count; i++) + { + p1.Execute(i); + } + + BuildImmediateSplit2To5(ref layer, bodies, aabbs, layerIndices, xMinMaxs); + } + + static void BuildImmediateAoS2To5(ref CollisionLayer layer, + NativeArray colliderAoS, + NativeArray layerIndices, + NativeArray xMinMaxs) + { new Part2Job { layer = layer, @@ -382,7 +612,7 @@ public static void BuildImmediate(ref CollisionLayer layer, NativeArray bodies, NativeArray aabbs) + static void BuildImmediateSplit2To5(ref CollisionLayer layer, + NativeArray bodies, + NativeArray aabbs, + NativeArray layerIndices, + NativeArray xMinMaxs) { - var layerIndices = new NativeArray(bodies.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - var xMinMaxs = new NativeArray(bodies.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - - var p1 = new Part1FromDualArraysJob - { - aabbs = aabbs, - layer = layer, - layerIndices = layerIndices, - xMinMaxs = xMinMaxs - }; - for (int i = 0; i < layer.count; i++) - { - p1.Execute(i); - } - new Part2Job { layer = layer, @@ -439,7 +657,7 @@ public static void BuildImmediate(ref CollisionLayer layer, NativeArray aabbs, int count) + { + if (aabbs.Length != count) + throw new InvalidOperationException( + $"The number of elements in overrideAbbs does not match the number of bodies in the bodies array"); + } + #endregion } } diff --git a/PsyshockPhysics/Physics/Internal/Builders/ConvexHullBuilder.cs b/PsyshockPhysics/Physics/Internal/Builders/ConvexHullBuilder.cs index 8c807db..7859ab0 100644 --- a/PsyshockPhysics/Physics/Internal/Builders/ConvexHullBuilder.cs +++ b/PsyshockPhysics/Physics/Internal/Builders/ConvexHullBuilder.cs @@ -591,7 +591,7 @@ public unsafe bool AddPoint(float3 point, uint userData = 0, bool force2D = fals numFrontTriangles++; Plane plane = ComputePlane(triangleIndex, true); - float distance = math.dot(plane.normal, floatPoint) + plane.distanceFromOrigin; + float distance = math.dot(plane.normal, floatPoint) + plane.distanceToOrigin; maxDistance = math.max(distance, maxDistance); } else @@ -1522,12 +1522,12 @@ public void Solve(float3 normal0, float3 normal1) if (maxAxis.x) { planeOk = Solve(m_count, m_sums.yzx, m_squareSums.yzx, m_productSums.yzx, out Plane plane); - this.plane = new Plane(plane.normal.zxy, plane.distanceFromOrigin); + this.plane = new Plane(plane.normal.zxy, plane.distanceToOrigin); } else if (maxAxis.y) { planeOk = Solve(m_count, m_sums.zxy, m_squareSums.zxy, m_productSums.zxy, out Plane plane); - this.plane = new Plane(plane.normal.yzx, plane.distanceFromOrigin); + this.plane = new Plane(plane.normal.yzx, plane.distanceToOrigin); } else { @@ -1746,7 +1746,7 @@ public unsafe float SimplifyFacesAndShrink(float simplificationTolerance, float int vertexIndex1 = neighborTriangle.GetVertex(neighborEdge.edgeIndex); int edgePlaneIndex = numPlanes + edgeIndex; Plane edgePlane = GetEdgePlane(vertexIndex0, vertexIndex1, normal0, normal1); - edgePlane.distanceFromOrigin -= simplificationTolerance / 2.0f; // push the edge plane out slightly so it only becomes active if the face planes change significiantly + edgePlane.distanceToOrigin -= simplificationTolerance / 2.0f; // push the edge plane out slightly so it only becomes active if the face planes change significiantly planes[edgePlaneIndex] = edgePlane; // Build its OLS data @@ -2030,7 +2030,7 @@ public unsafe float SimplifyFacesAndShrink(float simplificationTolerance, float } else { - planes[i] = new Plane(planes[i].normal, planes[i].distanceFromOrigin + offset); + planes[i] = new Plane(planes[i].normal, planes[i].distanceToOrigin + offset); } } @@ -2040,7 +2040,7 @@ public unsafe float SimplifyFacesAndShrink(float simplificationTolerance, float for (int i = 0; i < numPlanes - 1; i++) { Plane plane0 = planes[i]; - float3 point0 = -plane0.normal * plane0.distanceFromOrigin; // A point on plane0 + float3 point0 = -plane0.normal * plane0.distanceToOrigin; // A point on plane0 for (int j = i + 1; j < numPlanes; j++) { Plane plane1 = planes[j]; @@ -2107,7 +2107,7 @@ public unsafe float SimplifyFacesAndShrink(float simplificationTolerance, float } double invDet = 1.0f / det; x = - (float3)((planes[i].distanceFromOrigin * jkCross - planes[j].distanceFromOrigin * ikCross + planes[k].distanceFromOrigin * ijCross) * -invDet); + (float3)((planes[i].distanceToOrigin * jkCross - planes[j].distanceToOrigin * ikCross + planes[k].distanceToOrigin * ijCross) * -invDet); } // Test if the point is inside of all of the other planes @@ -2116,7 +2116,7 @@ public unsafe float SimplifyFacesAndShrink(float simplificationTolerance, float for (int l = 0; l < numPlanes; l++) { const float tolerance = 1e-5f; - if (math.dot(planes[l].normal, x) > tolerance - planes[l].distanceFromOrigin) + if (math.dot(planes[l].normal, x) > tolerance - planes[l].distanceToOrigin) { inside = false; break; diff --git a/PsyshockPhysics/Physics/Internal/Math/Plane.cs b/PsyshockPhysics/Physics/Internal/Math/Plane.cs index 5e30ed4..e4f8670 100644 --- a/PsyshockPhysics/Physics/Internal/Math/Plane.cs +++ b/PsyshockPhysics/Physics/Internal/Math/Plane.cs @@ -13,7 +13,8 @@ public float3 normal set => m_normalAndDistance.xyz = value; } - public float distanceFromOrigin + // Distance to the origin along the normal + public float distanceToOrigin { get => m_normalAndDistance.w; set => m_normalAndDistance.w = value; diff --git a/PsyshockPhysics/Physics/Internal/Math/mathex.collision.cs b/PsyshockPhysics/Physics/Internal/Math/mathex.collision.cs index 3baf695..852a6c6 100644 --- a/PsyshockPhysics/Physics/Internal/Math/mathex.collision.cs +++ b/PsyshockPhysics/Physics/Internal/Math/mathex.collision.cs @@ -42,6 +42,108 @@ public static void GetDualPerpendicularNormalized(float3 unscaledInput, out floa float3 cross = math.cross(v, dir); perpendicularB = cross * invLength; } + + public static float cproduct(float3 v) => v.x * v.y * v.z; + + internal static quaternion FromToRotation(float3 from, float3 to) + { + Unity.Assertions.Assert.IsTrue(math.abs(math.lengthsq(from) - 1.0f) < 1e-4f); + Unity.Assertions.Assert.IsTrue(math.abs(math.lengthsq(to) - 1.0f) < 1e-4f); + float3 cross = math.cross(from, to); + GetDualPerpendicularNormalized(from, out float3 safeAxis, out _); // for when angle ~= 180 + float dot = math.dot(from, to); + float3 squares = new float3(0.5f - new float2(dot, -dot) * 0.5f, math.lengthsq(cross)); + float3 inverses = math.select(math.rsqrt(squares), 0.0f, squares < 1e-10f); + float2 sinCosHalfAngle = squares.xy * inverses.xy; + float3 axis = math.select(cross * inverses.z, safeAxis, squares.z < 1e-10f); + return new quaternion(new float4(axis * sinCosHalfAngle.x, sinCosHalfAngle.y)); + } + + /// Calculate the eigenvectors and eigenvalues of a symmetric 3x3 matrix. + internal static void DiagonalizeSymmetricApproximation(float3x3 a, out float3x3 eigenVectors, out float3 eigenValues) + { + float GetMatrixElement(float3x3 m, int row, int col) + { + switch (col) + { + case 0: return m.c0[row]; + case 1: return m.c1[row]; + case 2: return m.c2[row]; + default: UnityEngine.Assertions.Assert.IsTrue(false); return 0.0f; + } + } + + void SetMatrixElement(ref float3x3 m, int row, int col, float x) + { + switch (col) + { + case 0: m.c0[row] = x; break; + case 1: m.c1[row] = x; break; + case 2: m.c2[row] = x; break; + default: UnityEngine.Assertions.Assert.IsTrue(false); break; + } + } + + eigenVectors = float3x3.identity; + float epsSq = 1e-14f * (math.lengthsq(a.c0) + math.lengthsq(a.c1) + math.lengthsq(a.c2)); + const int maxIterations = 10; + for (int iteration = 0; iteration < maxIterations; iteration++) + { + // Find the row (p) and column (q) of the off-diagonal entry with greater magnitude + int p = 0, q = 1; + { + float maxEntry = math.abs(a.c1[0]); + float mag02 = math.abs(a.c2[0]); + float mag12 = math.abs(a.c2[1]); + if (mag02 > maxEntry) + { + maxEntry = mag02; + p = 0; + q = 2; + } + if (mag12 > maxEntry) + { + maxEntry = mag12; + p = 1; + q = 2; + } + + // Terminate if it's small enough + if (maxEntry * maxEntry < epsSq) + { + break; + } + } + + // Calculate jacobia rotation + float3x3 j = float3x3.identity; + { + float apq = GetMatrixElement(a, p, q); + float tau = (GetMatrixElement(a, q, q) - GetMatrixElement(a, p, p)) / (2.0f * apq); + float t = math.sqrt(1.0f + tau * tau); + if (tau > 0.0f) + { + t = 1.0f / (tau + t); + } + else + { + t = 1.0f / (tau - t); + } + float c = math.rsqrt(1.0f + t * t); + float s = t * c; + + SetMatrixElement(ref j, p, p, c); + SetMatrixElement(ref j, q, q, c); + SetMatrixElement(ref j, p, q, s); + SetMatrixElement(ref j, q, p, -s); + } + + // Rotate a + a = math.mul(math.transpose(j), math.mul(a, j)); + eigenVectors = math.mul(eigenVectors, j); + } + eigenValues = new float3(a.c0.x, a.c1.y, a.c2.z); + } } } diff --git a/PsyshockPhysics/Physics/Internal/Math/mathex.plane.cs b/PsyshockPhysics/Physics/Internal/Math/mathex.plane.cs index c8ebe6e..734f924 100644 --- a/PsyshockPhysics/Physics/Internal/Math/mathex.plane.cs +++ b/PsyshockPhysics/Physics/Internal/Math/mathex.plane.cs @@ -32,7 +32,7 @@ public static Plane Flip(Plane plane) internal static Plane TransformPlane(RigidTransform transform, Plane plane) { float3 normal = math.rotate(transform.rot, plane.normal); - return new Plane(normal, plane.distanceFromOrigin - math.dot(normal, transform.pos)); + return new Plane(normal, plane.distanceToOrigin - math.dot(normal, transform.pos)); } } } diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxBox.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxBox.cs index 75f3a9e..9b3e6e2 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxBox.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxBox.cs @@ -184,7 +184,7 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in BoxCollider bEdgePlaneNormals = simd.mul(bInATransform.rot, bEdgePlaneNormals); var bEdgePlaneDistances = simd.dot(bEdgePlaneNormals, bVertices.bcda); bool needsClosestPoint = true; - var distanceScalarAlongContactNormalB = math.rcp(math.dot(aLocalContactNormal, bPlane.normal)); + var distanceScalarAlongContactNormalB = math.rcp(math.dot(-aLocalContactNormal, bPlane.normal)); // Project and clip edges of A onto the face of B. for (int edgeIndex = 0; edgeIndex < 4; edgeIndex++) @@ -226,7 +226,7 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in BoxCollider for (int i = 0; i < 4; i++) { var vertex = bVertices[i]; - if (math.all(simd.dot(aEdgePlaneNormals, vertex) + aEdgePlaneDistances <= 0f)) + if (math.all(simd.dot(aEdgePlaneNormals, vertex) < aEdgePlaneDistances)) { var distance = mathex.SignedDistance(aPlane, vertex) * distanceScalarAlongContactNormalA; result.Add(math.transform(aTransform, vertex), distance); diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxCompound.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxCompound.cs index 8d5149c..0c5a21b 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxCompound.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxCompound.cs @@ -157,9 +157,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return sphereResult; case ColliderType.Capsule: var capsuleResult = CapsuleBox.DistanceBetween(in box, @@ -168,9 +166,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return capsuleResult; case ColliderType.Box: return BoxBox.DistanceBetween(in collider.m_box, in colliderTransform, in box, in boxTransform, maxDistance, out result); diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxConvex.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxConvex.cs index 89aae98..ccd8521 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxConvex.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxConvex.cs @@ -15,7 +15,7 @@ public static bool DistanceBetween(in ConvexCollider convex, { var bInATransform = math.mul(math.inverse(convexTransform), boxTransform); var gjkResult = GjkEpa.DoGjkEpa(convex, box, in bInATransform); - var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(1e-4f, -1e-4f, gjkResult.distance < 0f); + var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(-1e-4f, 1e-4f, gjkResult.distance < 0f); SphereConvex.DistanceBetween(in convex, in RigidTransform.identity, new SphereCollider(gjkResult.hitpointOnAInASpace + epsilon, 0f), @@ -157,18 +157,20 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in ConvexColli out int faceIndex, out int edgeCount); PointRayBox.BestFacePlanesAndVertices(in box, bLocalContactNormal, out var bEdgePlaneNormals, out _, out var bPlane, out var bVertices); + bPlane = mathex.TransformPlane(bInATransform, bPlane); bVertices = simd.transform(bInATransform, bVertices); bEdgePlaneNormals = simd.mul(bInATransform.rot, bEdgePlaneNormals); var bEdgePlaneDistances = simd.dot(bEdgePlaneNormals, bVertices.bcda); bool needsClosestPoint = true; - var distanceScalarAlongContactNormalB = math.rcp(math.dot(aLocalContactNormal, bPlane.normal)); + var distanceScalarAlongContactNormalB = math.rcp(math.dot(-aLocalContactNormal, bPlane.normal)); bool projectBOnA = math.abs(math.dot(aPlane.normal, aLocalContactNormal)) < 0.05f; int4 positiveSideCounts = 0; int4 negativeSideCounts = 0; - UnityContactManifoldExtra3D result = default; - var edgeIndicesBase = blob.edgeIndicesInFacesStartsAndCounts[faceIndex].start; + UnityContactManifoldExtra3D result = default; + result.baseStorage.contactNormal = contactNormal; + var edgeIndicesBase = blob.edgeIndicesInFacesStartsAndCounts[faceIndex].start; // Project and clip edges of A onto the face of B. for (int edgeIndex = 0; edgeIndex < edgeCount; edgeIndex++) @@ -213,9 +215,9 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in ConvexColli { var aEdgePlaneNormal = math.cross(rayDisplacement, aLocalContactNormal); var edgePlaneDistance = math.dot(aEdgePlaneNormal, rayStart); - var projection = simd.dot(bVertices, aEdgePlaneNormal) + edgePlaneDistance; - positiveSideCounts += math.select(int4.zero, 1, projection > 0f); - negativeSideCounts += math.select(int4.zero, 1, projection < 0f); + var projection = simd.dot(bVertices, aEdgePlaneNormal); + positiveSideCounts += math.select(int4.zero, 1, projection > edgePlaneDistance); + negativeSideCounts += math.select(int4.zero, 1, projection < edgePlaneDistance); } } if (projectBOnA) @@ -300,13 +302,14 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in ConvexColli bEdgePlaneNormals = simd.mul(bInATransform.rot, bEdgePlaneNormals); var bEdgePlaneDistances = simd.dot(bEdgePlaneNormals, bVertices.bcda); bool needsClosestPoint = true; - var distanceScalarAlongContactNormalB = math.rcp(math.dot(aLocalContactNormal, bPlane.normal)); + var distanceScalarAlongContactNormalB = math.rcp(math.dot(-aLocalContactNormal, bPlane.normal)); bool projectBOnA = math.abs(math.dot(aPlane.normal, aLocalContactNormal)) < 0.05f; int4 positiveSideCounts = 0; int4 negativeSideCounts = 0; UnityContactManifoldExtra2D result = default; + result.baseStorage.contactNormal = contactNormal; // Project and clip edges of A onto the face of B. for (int edgeIndex = 0; edgeIndex < indices2D.Length; edgeIndex++) @@ -363,9 +366,9 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in ConvexColli { var aEdgePlaneNormal = math.cross(rayDisplacement, aLocalContactNormal); var edgePlaneDistance = math.dot(aEdgePlaneNormal, rayStart); - var projection = simd.dot(bVertices, aEdgePlaneNormal) + edgePlaneDistance; - positiveSideCounts += math.select(int4.zero, 1, projection > 0f); - negativeSideCounts += math.select(int4.zero, 1, projection < 0f); + var projection = simd.dot(bVertices, aEdgePlaneNormal); + positiveSideCounts += math.select(int4.zero, 1, projection > edgePlaneDistance); + negativeSideCounts += math.select(int4.zero, 1, projection < edgePlaneDistance); } } if (projectBOnA) diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxTriMesh.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxTriMesh.cs index 77d7155..155f2b4 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxTriMesh.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxTriMesh.cs @@ -14,14 +14,17 @@ public static bool DistanceBetween(in TriMeshCollider triMesh, float maxDistance, out ColliderDistanceResult result) { - var boxInTriMeshTransform = math.mul(math.inverse(triMeshTransform), boxTransform); - var aabb = Physics.AabbFrom(box, in boxInTriMeshTransform); - var processor = new BoxDistanceProcessor + var boxInTriMeshTransform = math.mul(math.inverse(triMeshTransform), boxTransform); + var aabb = Physics.AabbFrom(box, in boxInTriMeshTransform); + aabb.min -= maxDistance; + aabb.max += maxDistance; + var processor = new BoxDistanceProcessor { blob = triMesh.triMeshColliderBlob, box = box, boxTransform = boxInTriMeshTransform, maxDistance = maxDistance, + bestDistance = float.MaxValue, found = false, scale = triMesh.scale }; @@ -49,9 +52,11 @@ public static unsafe void DistanceBetweenAll(in TriMeshCollider triMesh, float maxDistance, ref T processor) where T : unmanaged, IDistanceBetweenAllProcessor { - var boxInTriMeshTransform = math.mul(math.inverse(triMeshTransform), boxTransform); - var aabb = Physics.AabbFrom(box, boxInTriMeshTransform); - var triProcessor = new DistanceAllProcessor + var boxInTriMeshTransform = math.mul(math.inverse(triMeshTransform), boxTransform); + var aabb = Physics.AabbFrom(box, boxInTriMeshTransform); + aabb.min -= maxDistance; + aabb.max += maxDistance; + var triProcessor = new DistanceAllProcessor { triMesh = triMesh, triMeshTransform = triMeshTransform, diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxTriangle.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxTriangle.cs index c5911c5..4185f13 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxTriangle.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/BoxTriangle.cs @@ -15,15 +15,15 @@ public static bool DistanceBetween(in TriangleCollider triangle, // Todo: SAT algorithm similar to box vs box. var bInATransform = math.mul(math.inverse(triangleTransform), boxTransform); var gjkResult = GjkEpa.DoGjkEpa(triangle, box, in bInATransform); - var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(1e-4f, -1e-4f, gjkResult.distance < 0f); + var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(-1e-4f, 1e-4f, gjkResult.distance < 0f); SphereTriangle.DistanceBetween(in triangle, - in triangleTransform, + in RigidTransform.identity, new SphereCollider(gjkResult.hitpointOnAInASpace + epsilon, 0f), RigidTransform.identity, float.MaxValue, out var closestOnA); SphereBox.DistanceBetween(in box, - in boxTransform, + in bInATransform, new SphereCollider(gjkResult.hitpointOnBInASpace - epsilon, 0f), RigidTransform.identity, float.MaxValue, @@ -241,7 +241,7 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in TriangleCol out var aVertices); PointRayBox.BestFacePlanesAndVertices(in box, bLocalContactNormal, out var bEdgePlaneNormals, out var bEdgePlaneDistances, out var bPlane, out var bVertices); bool needsClosestPoint = true; - var distanceScalarAlongContactNormalB = math.rcp(math.dot(aLocalContactNormal, bPlane.normal)); + var distanceScalarAlongContactNormalB = math.rcp(math.dot(-aLocalContactNormal, bPlane.normal)); // Project and clip edges of A onto the face of B. for (int edgeIndex = 0; edgeIndex < 3; edgeIndex++) @@ -285,7 +285,7 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in TriangleCol for (int i = 0; i < 4; i++) { var vertex = bVertices[i]; - if (math.all(simd.dot(aEdgePlaneNormals, vertex) + aEdgePlaneDistances <= 0f)) + if (math.all(simd.dot(aEdgePlaneNormals, vertex) < aEdgePlaneDistances)) { var distance = mathex.SignedDistance(aPlane, vertex) * distanceScalarAlongContactNormalA; result.Add(math.transform(boxTransform, vertex), distance); diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleCompound.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleCompound.cs index 4030fe3..c279729 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleCompound.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleCompound.cs @@ -156,9 +156,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return sphereResult; case ColliderType.Capsule: return CapsuleCapsule.DistanceBetween(in collider.m_capsule, in colliderTransform, in capsule, in capsuleTransform, maxDistance, out result); diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleConvex.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleConvex.cs index 5dcae9e..bf92b32 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleConvex.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleConvex.cs @@ -15,7 +15,7 @@ public static bool DistanceBetween(in ConvexCollider convex, { var bInATransform = math.mul(math.inverse(convexTransform), capsuleTransform); var gjkResult = GjkEpa.DoGjkEpa(convex, capsule, in bInATransform); - var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(1e-4f, -1e-4f, gjkResult.distance < 0f); + var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(-1e-4f, 1e-4f, gjkResult.distance < 0f); SphereConvex.DistanceBetween(in convex, in RigidTransform.identity, new SphereCollider(gjkResult.hitpointOnAInASpace + epsilon, 0f), @@ -93,7 +93,7 @@ public static bool ColliderCast(in CapsuleCollider capsuleToCast, float4 obbX = new float4(planeA.normal.x, planeB.normal.x, planeC.normal.x, planeD.normal.x); float4 obbY = new float4(planeA.normal.y, planeB.normal.y, planeC.normal.y, planeD.normal.y); float4 obbZ = new float4(planeA.normal.z, planeB.normal.z, planeC.normal.z, planeD.normal.z); - float4 obbD = new float4(planeA.distanceFromOrigin, planeB.distanceFromOrigin, planeC.distanceFromOrigin, planeD.distanceFromOrigin); + float4 obbD = new float4(planeA.distanceToOrigin, planeB.distanceToOrigin, planeC.distanceToOrigin, planeD.distanceToOrigin); for (int i = 0; i < blob.vertexIndicesInEdges.Length; i++) { @@ -197,7 +197,7 @@ public static bool ColliderCast(in ConvexCollider convexToCast, float4 obbX = new float4(planeA.normal.x, planeB.normal.x, planeC.normal.x, planeD.normal.x); float4 obbY = new float4(planeA.normal.y, planeB.normal.y, planeC.normal.y, planeD.normal.y); float4 obbZ = new float4(planeA.normal.z, planeB.normal.z, planeC.normal.z, planeD.normal.z); - float4 obbD = new float4(planeA.distanceFromOrigin, planeB.distanceFromOrigin, planeC.distanceFromOrigin, planeD.distanceFromOrigin); + float4 obbD = new float4(planeA.distanceToOrigin, planeB.distanceToOrigin, planeC.distanceToOrigin, planeD.distanceToOrigin); for (int i = 0; i < blob.vertexIndicesInEdges.Length; i++) { diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleTriMesh.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleTriMesh.cs index 049be73..177b632 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleTriMesh.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CapsuleTriMesh.cs @@ -18,14 +18,17 @@ public static bool DistanceBetween(in TriMeshCollider triMesh, var capsuleInTriMesh = new CapsuleCollider(math.transform(capInTriMeshTransform, capsule.pointA), math.transform(capInTriMeshTransform, capsule.pointB), capsule.radius); - var aabb = Physics.AabbFrom(capsuleInTriMesh, RigidTransform.identity); - var processor = new CapsuleDistanceProcessor + var aabb = Physics.AabbFrom(capsuleInTriMesh, RigidTransform.identity); + aabb.min -= maxDistance; + aabb.max += maxDistance; + var processor = new CapsuleDistanceProcessor { - blob = triMesh.triMeshColliderBlob, - capsule = capsuleInTriMesh, - maxDistance = maxDistance, - found = false, - scale = triMesh.scale + blob = triMesh.triMeshColliderBlob, + capsule = capsuleInTriMesh, + maxDistance = maxDistance, + bestDistance = float.MaxValue, + found = false, + scale = triMesh.scale }; triMesh.triMeshColliderBlob.Value.FindTriangles(in aabb, ref processor, triMesh.scale); if (processor.found) @@ -46,9 +49,11 @@ public static unsafe void DistanceBetweenAll(in TriMeshCollider triMesh, float maxDistance, ref T processor) where T : unmanaged, IDistanceBetweenAllProcessor { - var capsuleInTriMeshTransform = math.mul(math.inverse(triMeshTransform), capsuleTransform); - var aabb = Physics.AabbFrom(capsule, capsuleInTriMeshTransform); - var triProcessor = new DistanceAllProcessor + var capsuleInTriMeshTransform = math.mul(math.inverse(triMeshTransform), capsuleTransform); + var aabb = Physics.AabbFrom(capsule, capsuleInTriMeshTransform); + aabb.min -= maxDistance; + aabb.max += maxDistance; + var triProcessor = new DistanceAllProcessor { triMesh = triMesh, triMeshTransform = triMeshTransform, diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ColliderColliderDispatch.gen.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ColliderColliderDispatch.gen.cs index 5443c6d..4e1b0e5 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ColliderColliderDispatch.gen.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ColliderColliderDispatch.gen.cs @@ -43,13 +43,13 @@ public static bool DistanceBetween(in Collider colliderA, } case (ColliderType.Sphere, ColliderType.TriMesh): { - var r = SphereTriMesh.DistanceBetween(in colliderB.m_triMesh, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result); + var r = SphereTriMesh.DistanceBetween(in colliderB.m_triMesh(), in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } case (ColliderType.Sphere, ColliderType.Compound): { - var r = SphereCompound.DistanceBetween(in colliderB.m_compound, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result); + var r = SphereCompound.DistanceBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } @@ -77,13 +77,13 @@ public static bool DistanceBetween(in Collider colliderA, } case (ColliderType.Capsule, ColliderType.TriMesh): { - var r = CapsuleTriMesh.DistanceBetween(in colliderB.m_triMesh, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result); + var r = CapsuleTriMesh.DistanceBetween(in colliderB.m_triMesh(), in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } case (ColliderType.Capsule, ColliderType.Compound): { - var r = CapsuleCompound.DistanceBetween(in colliderB.m_compound, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result); + var r = CapsuleCompound.DistanceBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } @@ -107,13 +107,13 @@ public static bool DistanceBetween(in Collider colliderA, } case (ColliderType.Box, ColliderType.TriMesh): { - var r = BoxTriMesh.DistanceBetween(in colliderB.m_triMesh, in bTransform, in colliderA.m_box, in aTransform, maxDistance, out result); + var r = BoxTriMesh.DistanceBetween(in colliderB.m_triMesh(), in bTransform, in colliderA.m_box, in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } case (ColliderType.Box, ColliderType.Compound): { - var r = BoxCompound.DistanceBetween(in colliderB.m_compound, in bTransform, in colliderA.m_box, in aTransform, maxDistance, out result); + var r = BoxCompound.DistanceBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_box, in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } @@ -133,13 +133,13 @@ public static bool DistanceBetween(in Collider colliderA, } case (ColliderType.Triangle, ColliderType.TriMesh): { - var r = TriangleTriMesh.DistanceBetween(in colliderB.m_triMesh, in bTransform, in colliderA.m_triangle, in aTransform, maxDistance, out result); + var r = TriangleTriMesh.DistanceBetween(in colliderB.m_triMesh(), in bTransform, in colliderA.m_triangle, in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } case (ColliderType.Triangle, ColliderType.Compound): { - var r = TriangleCompound.DistanceBetween(in colliderB.m_compound, in bTransform, in colliderA.m_triangle, in aTransform, maxDistance, out result); + var r = TriangleCompound.DistanceBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_triangle, in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } @@ -155,48 +155,48 @@ public static bool DistanceBetween(in Collider colliderA, return ConvexConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_convex, in bTransform, maxDistance, out result); case (ColliderType.Convex, ColliderType.TriMesh): { - var r = ConvexTriMesh.DistanceBetween(in colliderB.m_triMesh, in bTransform, in colliderA.m_convex, in aTransform, maxDistance, out result); + var r = ConvexTriMesh.DistanceBetween(in colliderB.m_triMesh(), in bTransform, in colliderA.m_convex, in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } case (ColliderType.Convex, ColliderType.Compound): { - var r = ConvexCompound.DistanceBetween(in colliderB.m_compound, in bTransform, in colliderA.m_convex, in aTransform, maxDistance, out result); + var r = ConvexCompound.DistanceBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_convex, in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } case (ColliderType.TriMesh, ColliderType.Sphere): - return SphereTriMesh.DistanceBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); + return SphereTriMesh.DistanceBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); case (ColliderType.TriMesh, ColliderType.Capsule): - return CapsuleTriMesh.DistanceBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result); + return CapsuleTriMesh.DistanceBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result); case (ColliderType.TriMesh, ColliderType.Box): - return BoxTriMesh.DistanceBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_box, in bTransform, maxDistance, out result); + return BoxTriMesh.DistanceBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_box, in bTransform, maxDistance, out result); case (ColliderType.TriMesh, ColliderType.Triangle): - return TriangleTriMesh.DistanceBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, out result); + return TriangleTriMesh.DistanceBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, out result); case (ColliderType.TriMesh, ColliderType.Convex): - return ConvexTriMesh.DistanceBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_convex, in bTransform, maxDistance, out result); + return ConvexTriMesh.DistanceBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_convex, in bTransform, maxDistance, out result); case (ColliderType.TriMesh, ColliderType.TriMesh): - return TriMeshTriMesh.DistanceBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_triMesh, in bTransform, maxDistance, out result); + return TriMeshTriMesh.DistanceBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_triMesh(), in bTransform, maxDistance, out result); case (ColliderType.TriMesh, ColliderType.Compound): { - var r = TriMeshCompound.DistanceBetween(in colliderB.m_compound, in bTransform, in colliderA.m_triMesh, in aTransform, maxDistance, out result); + var r = TriMeshCompound.DistanceBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_triMesh(), in aTransform, maxDistance, out result); result.FlipInPlace(); return r; } case (ColliderType.Compound, ColliderType.Sphere): - return SphereCompound.DistanceBetween(in colliderA.m_compound, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); + return SphereCompound.DistanceBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); case (ColliderType.Compound, ColliderType.Capsule): - return CapsuleCompound.DistanceBetween(in colliderA.m_compound, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result); + return CapsuleCompound.DistanceBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result); case (ColliderType.Compound, ColliderType.Box): - return BoxCompound.DistanceBetween(in colliderA.m_compound, in aTransform, in colliderB.m_box, in bTransform, maxDistance, out result); + return BoxCompound.DistanceBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_box, in bTransform, maxDistance, out result); case (ColliderType.Compound, ColliderType.Triangle): - return TriangleCompound.DistanceBetween(in colliderA.m_compound, in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, out result); + return TriangleCompound.DistanceBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, out result); case (ColliderType.Compound, ColliderType.Convex): - return ConvexCompound.DistanceBetween(in colliderA.m_compound, in aTransform, in colliderB.m_convex, in bTransform, maxDistance, out result); + return ConvexCompound.DistanceBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_convex, in bTransform, maxDistance, out result); case (ColliderType.Compound, ColliderType.TriMesh): - return TriMeshCompound.DistanceBetween(in colliderA.m_compound, in aTransform, in colliderB.m_triMesh, in bTransform, maxDistance, out result); + return TriMeshCompound.DistanceBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_triMesh(), in bTransform, maxDistance, out result); case (ColliderType.Compound, ColliderType.Compound): - return CompoundCompound.DistanceBetween(in colliderA.m_compound, in aTransform, in colliderB.m_compound, in bTransform, maxDistance, out result); + return CompoundCompound.DistanceBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_compound(), in bTransform, maxDistance, out result); default: result = default; return false; @@ -221,241 +221,260 @@ public static unsafe void DistanceBetweenAll(in Collider colliderA, ref T processor) where T : unmanaged, IDistanceBetweenAllProcessor { var flipper = new DistanceAllResultFlipper { processor = (T*)UnsafeUtility.AddressOf(ref processor) }; - ColliderDistanceResult result = default; - + ColliderDistanceResult result; switch ((colliderA.type, colliderB.type)) { case (ColliderType.Sphere, ColliderType.Sphere): { - SphereSphere.DistanceBetween(in colliderA.m_sphere, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (SphereSphere.DistanceBetween(in colliderA.m_sphere, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Sphere, ColliderType.Capsule): { - SphereCapsule.DistanceBetween(in colliderB.m_capsule, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (SphereCapsule.DistanceBetween(in colliderB.m_capsule, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } case (ColliderType.Sphere, ColliderType.Box): { - SphereBox.DistanceBetween(in colliderB.m_box, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (SphereBox.DistanceBetween(in colliderB.m_box, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } case (ColliderType.Sphere, ColliderType.Triangle): { - SphereTriangle.DistanceBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (SphereTriangle.DistanceBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } case (ColliderType.Sphere, ColliderType.Convex): { - SphereConvex.DistanceBetween(in colliderB.m_convex, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (SphereConvex.DistanceBetween(in colliderB.m_convex, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } case (ColliderType.Sphere, ColliderType.TriMesh): - SphereTriMesh.DistanceBetweenAll(in colliderB.m_triMesh, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, ref flipper); + SphereTriMesh.DistanceBetweenAll(in colliderB.m_triMesh(), in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, ref flipper); break; case (ColliderType.Sphere, ColliderType.Compound): - SphereCompound.DistanceBetweenAll(in colliderB.m_compound, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, ref flipper); + SphereCompound.DistanceBetweenAll(in colliderB.m_compound(), in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, ref flipper); break; case (ColliderType.Capsule, ColliderType.Sphere): { - SphereCapsule.DistanceBetween(in colliderA.m_capsule, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (SphereCapsule.DistanceBetween(in colliderA.m_capsule, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Capsule, ColliderType.Capsule): { - CapsuleCapsule.DistanceBetween(in colliderA.m_capsule, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (CapsuleCapsule.DistanceBetween(in colliderA.m_capsule, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Capsule, ColliderType.Box): { - CapsuleBox.DistanceBetween(in colliderB.m_box, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (CapsuleBox.DistanceBetween(in colliderB.m_box, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } case (ColliderType.Capsule, ColliderType.Triangle): { - CapsuleTriangle.DistanceBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (CapsuleTriangle.DistanceBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } case (ColliderType.Capsule, ColliderType.Convex): { - CapsuleConvex.DistanceBetween(in colliderB.m_convex, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (CapsuleConvex.DistanceBetween(in colliderB.m_convex, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } case (ColliderType.Capsule, ColliderType.TriMesh): - CapsuleTriMesh.DistanceBetweenAll(in colliderB.m_triMesh, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, ref flipper); + CapsuleTriMesh.DistanceBetweenAll(in colliderB.m_triMesh(), in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, ref flipper); break; case (ColliderType.Capsule, ColliderType.Compound): - CapsuleCompound.DistanceBetweenAll(in colliderB.m_compound, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, ref flipper); + CapsuleCompound.DistanceBetweenAll(in colliderB.m_compound(), in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, ref flipper); break; case (ColliderType.Box, ColliderType.Sphere): { - SphereBox.DistanceBetween(in colliderA.m_box, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (SphereBox.DistanceBetween(in colliderA.m_box, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Box, ColliderType.Capsule): { - CapsuleBox.DistanceBetween(in colliderA.m_box, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (CapsuleBox.DistanceBetween(in colliderA.m_box, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Box, ColliderType.Box): { - BoxBox.DistanceBetween(in colliderA.m_box, in aTransform, in colliderB.m_box, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (BoxBox.DistanceBetween(in colliderA.m_box, in aTransform, in colliderB.m_box, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Box, ColliderType.Triangle): { - BoxTriangle.DistanceBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_box, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (BoxTriangle.DistanceBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_box, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } case (ColliderType.Box, ColliderType.Convex): { - BoxConvex.DistanceBetween(in colliderB.m_convex, in bTransform, in colliderA.m_box, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (BoxConvex.DistanceBetween(in colliderB.m_convex, in bTransform, in colliderA.m_box, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } case (ColliderType.Box, ColliderType.TriMesh): - BoxTriMesh.DistanceBetweenAll(in colliderB.m_triMesh, in bTransform, in colliderA.m_box, in aTransform, maxDistance, ref flipper); + BoxTriMesh.DistanceBetweenAll(in colliderB.m_triMesh(), in bTransform, in colliderA.m_box, in aTransform, maxDistance, ref flipper); break; case (ColliderType.Box, ColliderType.Compound): - BoxCompound.DistanceBetweenAll(in colliderB.m_compound, in bTransform, in colliderA.m_box, in aTransform, maxDistance, ref flipper); + BoxCompound.DistanceBetweenAll(in colliderB.m_compound(), in bTransform, in colliderA.m_box, in aTransform, maxDistance, ref flipper); break; case (ColliderType.Triangle, ColliderType.Sphere): { - SphereTriangle.DistanceBetween(in colliderA.m_triangle, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (SphereTriangle.DistanceBetween(in colliderA.m_triangle, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Triangle, ColliderType.Capsule): { - CapsuleTriangle.DistanceBetween(in colliderA.m_triangle, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (CapsuleTriangle.DistanceBetween(in colliderA.m_triangle, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Triangle, ColliderType.Box): { - BoxTriangle.DistanceBetween(in colliderA.m_triangle, in aTransform, in colliderB.m_box, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (BoxTriangle.DistanceBetween(in colliderA.m_triangle, in aTransform, in colliderB.m_box, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Triangle, ColliderType.Triangle): { - TriangleTriangle.DistanceBetween(in colliderA.m_triangle, in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (TriangleTriangle.DistanceBetween(in colliderA.m_triangle, in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Triangle, ColliderType.Convex): { - TriangleConvex.DistanceBetween(in colliderB.m_convex, in bTransform, in colliderA.m_triangle, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (TriangleConvex.DistanceBetween(in colliderB.m_convex, in bTransform, in colliderA.m_triangle, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } case (ColliderType.Triangle, ColliderType.TriMesh): - TriangleTriMesh.DistanceBetweenAll(in colliderB.m_triMesh, in bTransform, in colliderA.m_triangle, in aTransform, maxDistance, ref flipper); + TriangleTriMesh.DistanceBetweenAll(in colliderB.m_triMesh(), in bTransform, in colliderA.m_triangle, in aTransform, maxDistance, ref flipper); break; case (ColliderType.Triangle, ColliderType.Compound): - TriangleCompound.DistanceBetweenAll(in colliderB.m_compound, in bTransform, in colliderA.m_triangle, in aTransform, maxDistance, ref flipper); + TriangleCompound.DistanceBetweenAll(in colliderB.m_compound(), in bTransform, in colliderA.m_triangle, in aTransform, maxDistance, ref flipper); break; case (ColliderType.Convex, ColliderType.Sphere): { - SphereConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (SphereConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Convex, ColliderType.Capsule): { - CapsuleConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (CapsuleConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Convex, ColliderType.Box): { - BoxConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_box, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (BoxConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_box, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Convex, ColliderType.Triangle): { - TriangleConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (TriangleConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Convex, ColliderType.Convex): { - ConvexConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_convex, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (ConvexConvex.DistanceBetween(in colliderA.m_convex, in aTransform, in colliderB.m_convex, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } case (ColliderType.Convex, ColliderType.TriMesh): - ConvexTriMesh.DistanceBetweenAll(in colliderB.m_triMesh, in bTransform, in colliderA.m_convex, in aTransform, maxDistance, ref flipper); + ConvexTriMesh.DistanceBetweenAll(in colliderB.m_triMesh(), in bTransform, in colliderA.m_convex, in aTransform, maxDistance, ref flipper); break; case (ColliderType.Convex, ColliderType.Compound): - ConvexCompound.DistanceBetweenAll(in colliderB.m_compound, in bTransform, in colliderA.m_convex, in aTransform, maxDistance, ref flipper); + ConvexCompound.DistanceBetweenAll(in colliderB.m_compound(), in bTransform, in colliderA.m_convex, in aTransform, maxDistance, ref flipper); break; case (ColliderType.TriMesh, ColliderType.Sphere): - SphereTriMesh.DistanceBetweenAll(in colliderA.m_triMesh, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, ref processor); + SphereTriMesh.DistanceBetweenAll(in colliderA.m_triMesh(), in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, ref processor); break; case (ColliderType.TriMesh, ColliderType.Capsule): - CapsuleTriMesh.DistanceBetweenAll(in colliderA.m_triMesh, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, ref processor); + CapsuleTriMesh.DistanceBetweenAll(in colliderA.m_triMesh(), in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, ref processor); break; case (ColliderType.TriMesh, ColliderType.Box): - BoxTriMesh.DistanceBetweenAll(in colliderA.m_triMesh, in aTransform, in colliderB.m_box, in bTransform, maxDistance, ref processor); + BoxTriMesh.DistanceBetweenAll(in colliderA.m_triMesh(), in aTransform, in colliderB.m_box, in bTransform, maxDistance, ref processor); break; case (ColliderType.TriMesh, ColliderType.Triangle): - TriangleTriMesh.DistanceBetweenAll(in colliderA.m_triMesh, in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, ref processor); + TriangleTriMesh.DistanceBetweenAll(in colliderA.m_triMesh(), in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, ref processor); break; case (ColliderType.TriMesh, ColliderType.Convex): - ConvexTriMesh.DistanceBetweenAll(in colliderA.m_triMesh, in aTransform, in colliderB.m_convex, in bTransform, maxDistance, ref processor); + ConvexTriMesh.DistanceBetweenAll(in colliderA.m_triMesh(), in aTransform, in colliderB.m_convex, in bTransform, maxDistance, ref processor); break; case (ColliderType.TriMesh, ColliderType.TriMesh): - TriMeshTriMesh.DistanceBetweenAll(in colliderA.m_triMesh, in aTransform, in colliderB.m_triMesh, in bTransform, maxDistance, ref processor); + TriMeshTriMesh.DistanceBetweenAll(in colliderA.m_triMesh(), in aTransform, in colliderB.m_triMesh(), in bTransform, maxDistance, ref processor); break; case (ColliderType.TriMesh, ColliderType.Compound): - TriMeshCompound.DistanceBetweenAll(in colliderB.m_compound, in bTransform, in colliderA.m_triMesh, in aTransform, maxDistance, ref flipper); + TriMeshCompound.DistanceBetweenAll(in colliderB.m_compound(), in bTransform, in colliderA.m_triMesh(), in aTransform, maxDistance, ref flipper); break; case (ColliderType.Compound, ColliderType.Sphere): - SphereCompound.DistanceBetweenAll(in colliderA.m_compound, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, ref processor); + SphereCompound.DistanceBetweenAll(in colliderA.m_compound(), in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, ref processor); break; case (ColliderType.Compound, ColliderType.Capsule): - CapsuleCompound.DistanceBetweenAll(in colliderA.m_compound, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, ref processor); + CapsuleCompound.DistanceBetweenAll(in colliderA.m_compound(), in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, ref processor); break; case (ColliderType.Compound, ColliderType.Box): - BoxCompound.DistanceBetweenAll(in colliderA.m_compound, in aTransform, in colliderB.m_box, in bTransform, maxDistance, ref processor); + BoxCompound.DistanceBetweenAll(in colliderA.m_compound(), in aTransform, in colliderB.m_box, in bTransform, maxDistance, ref processor); break; case (ColliderType.Compound, ColliderType.Triangle): - TriangleCompound.DistanceBetweenAll(in colliderA.m_compound, in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, ref processor); + TriangleCompound.DistanceBetweenAll(in colliderA.m_compound(), in aTransform, in colliderB.m_triangle, in bTransform, maxDistance, ref processor); break; case (ColliderType.Compound, ColliderType.Convex): - ConvexCompound.DistanceBetweenAll(in colliderA.m_compound, in aTransform, in colliderB.m_convex, in bTransform, maxDistance, ref processor); + ConvexCompound.DistanceBetweenAll(in colliderA.m_compound(), in aTransform, in colliderB.m_convex, in bTransform, maxDistance, ref processor); break; case (ColliderType.Compound, ColliderType.TriMesh): - TriMeshCompound.DistanceBetweenAll(in colliderA.m_compound, in aTransform, in colliderB.m_triMesh, in bTransform, maxDistance, ref processor); + TriMeshCompound.DistanceBetweenAll(in colliderA.m_compound(), in aTransform, in colliderB.m_triMesh(), in bTransform, maxDistance, ref processor); break; case (ColliderType.Compound, ColliderType.Compound): - CompoundCompound.DistanceBetweenAll(in colliderA.m_compound, in aTransform, in colliderB.m_compound, in bTransform, maxDistance, ref processor); + CompoundCompound.DistanceBetweenAll(in colliderA.m_compound(), in aTransform, in colliderB.m_compound(), in bTransform, maxDistance, ref processor); break; } } @@ -480,9 +499,9 @@ public static bool ColliderCast(in Collider colliderToCast, case (ColliderType.Sphere, ColliderType.Convex): return SphereConvex.ColliderCast(in colliderToCast.m_sphere, in castStart, castEnd, in target.m_convex, in targetTransform, out result); case (ColliderType.Sphere, ColliderType.TriMesh): - return SphereTriMesh.ColliderCast(in colliderToCast.m_sphere, in castStart, castEnd, in target.m_triMesh, in targetTransform, out result); + return SphereTriMesh.ColliderCast(in colliderToCast.m_sphere, in castStart, castEnd, in target.m_triMesh(), in targetTransform, out result); case (ColliderType.Sphere, ColliderType.Compound): - return SphereCompound.ColliderCast(in colliderToCast.m_sphere, in castStart, castEnd, in target.m_compound, in targetTransform, out result); + return SphereCompound.ColliderCast(in colliderToCast.m_sphere, in castStart, castEnd, in target.m_compound(), in targetTransform, out result); case (ColliderType.Capsule, ColliderType.Sphere): return SphereCapsule.ColliderCast(in colliderToCast.m_capsule, in castStart, castEnd, in target.m_sphere, in targetTransform, out result); case (ColliderType.Capsule, ColliderType.Capsule): @@ -494,9 +513,9 @@ public static bool ColliderCast(in Collider colliderToCast, case (ColliderType.Capsule, ColliderType.Convex): return CapsuleConvex.ColliderCast(in colliderToCast.m_capsule, in castStart, castEnd, in target.m_convex, in targetTransform, out result); case (ColliderType.Capsule, ColliderType.TriMesh): - return CapsuleTriMesh.ColliderCast(in colliderToCast.m_capsule, in castStart, castEnd, in target.m_triMesh, in targetTransform, out result); + return CapsuleTriMesh.ColliderCast(in colliderToCast.m_capsule, in castStart, castEnd, in target.m_triMesh(), in targetTransform, out result); case (ColliderType.Capsule, ColliderType.Compound): - return CapsuleCompound.ColliderCast(in colliderToCast.m_capsule, in castStart, castEnd, in target.m_compound, in targetTransform, out result); + return CapsuleCompound.ColliderCast(in colliderToCast.m_capsule, in castStart, castEnd, in target.m_compound(), in targetTransform, out result); case (ColliderType.Box, ColliderType.Sphere): return SphereBox.ColliderCast(in colliderToCast.m_box, in castStart, castEnd, in target.m_sphere, in targetTransform, out result); case (ColliderType.Box, ColliderType.Capsule): @@ -508,9 +527,9 @@ public static bool ColliderCast(in Collider colliderToCast, case (ColliderType.Box, ColliderType.Convex): return BoxConvex.ColliderCast(in colliderToCast.m_box, in castStart, castEnd, in target.m_convex, in targetTransform, out result); case (ColliderType.Box, ColliderType.TriMesh): - return BoxTriMesh.ColliderCast(in colliderToCast.m_box, in castStart, castEnd, in target.m_triMesh, in targetTransform, out result); + return BoxTriMesh.ColliderCast(in colliderToCast.m_box, in castStart, castEnd, in target.m_triMesh(), in targetTransform, out result); case (ColliderType.Box, ColliderType.Compound): - return BoxCompound.ColliderCast(in colliderToCast.m_box, in castStart, castEnd, in target.m_compound, in targetTransform, out result); + return BoxCompound.ColliderCast(in colliderToCast.m_box, in castStart, castEnd, in target.m_compound(), in targetTransform, out result); case (ColliderType.Triangle, ColliderType.Sphere): return SphereTriangle.ColliderCast(in colliderToCast.m_triangle, in castStart, castEnd, in target.m_sphere, in targetTransform, out result); case (ColliderType.Triangle, ColliderType.Capsule): @@ -522,9 +541,9 @@ public static bool ColliderCast(in Collider colliderToCast, case (ColliderType.Triangle, ColliderType.Convex): return TriangleConvex.ColliderCast(in colliderToCast.m_triangle, in castStart, castEnd, in target.m_convex, in targetTransform, out result); case (ColliderType.Triangle, ColliderType.TriMesh): - return TriangleTriMesh.ColliderCast(in colliderToCast.m_triangle, in castStart, castEnd, in target.m_triMesh, in targetTransform, out result); + return TriangleTriMesh.ColliderCast(in colliderToCast.m_triangle, in castStart, castEnd, in target.m_triMesh(), in targetTransform, out result); case (ColliderType.Triangle, ColliderType.Compound): - return TriangleCompound.ColliderCast(in colliderToCast.m_triangle, in castStart, castEnd, in target.m_compound, in targetTransform, out result); + return TriangleCompound.ColliderCast(in colliderToCast.m_triangle, in castStart, castEnd, in target.m_compound(), in targetTransform, out result); case (ColliderType.Convex, ColliderType.Sphere): return SphereConvex.ColliderCast(in colliderToCast.m_convex, in castStart, castEnd, in target.m_sphere, in targetTransform, out result); case (ColliderType.Convex, ColliderType.Capsule): @@ -536,37 +555,37 @@ public static bool ColliderCast(in Collider colliderToCast, case (ColliderType.Convex, ColliderType.Convex): return ConvexConvex.ColliderCast(in colliderToCast.m_convex, in castStart, castEnd, in target.m_convex, in targetTransform, out result); case (ColliderType.Convex, ColliderType.TriMesh): - return ConvexTriMesh.ColliderCast(in colliderToCast.m_convex, in castStart, castEnd, in target.m_triMesh, in targetTransform, out result); + return ConvexTriMesh.ColliderCast(in colliderToCast.m_convex, in castStart, castEnd, in target.m_triMesh(), in targetTransform, out result); case (ColliderType.Convex, ColliderType.Compound): - return ConvexCompound.ColliderCast(in colliderToCast.m_convex, in castStart, castEnd, in target.m_compound, in targetTransform, out result); + return ConvexCompound.ColliderCast(in colliderToCast.m_convex, in castStart, castEnd, in target.m_compound(), in targetTransform, out result); case (ColliderType.TriMesh, ColliderType.Sphere): - return SphereTriMesh.ColliderCast(in colliderToCast.m_triMesh, in castStart, castEnd, in target.m_sphere, in targetTransform, out result); + return SphereTriMesh.ColliderCast(in colliderToCast.m_triMesh(), in castStart, castEnd, in target.m_sphere, in targetTransform, out result); case (ColliderType.TriMesh, ColliderType.Capsule): - return CapsuleTriMesh.ColliderCast(in colliderToCast.m_triMesh, in castStart, castEnd, in target.m_capsule, in targetTransform, out result); + return CapsuleTriMesh.ColliderCast(in colliderToCast.m_triMesh(), in castStart, castEnd, in target.m_capsule, in targetTransform, out result); case (ColliderType.TriMesh, ColliderType.Box): - return BoxTriMesh.ColliderCast(in colliderToCast.m_triMesh, in castStart, castEnd, in target.m_box, in targetTransform, out result); + return BoxTriMesh.ColliderCast(in colliderToCast.m_triMesh(), in castStart, castEnd, in target.m_box, in targetTransform, out result); case (ColliderType.TriMesh, ColliderType.Triangle): - return TriangleTriMesh.ColliderCast(in colliderToCast.m_triMesh, in castStart, castEnd, in target.m_triangle, in targetTransform, out result); + return TriangleTriMesh.ColliderCast(in colliderToCast.m_triMesh(), in castStart, castEnd, in target.m_triangle, in targetTransform, out result); case (ColliderType.TriMesh, ColliderType.Convex): - return ConvexTriMesh.ColliderCast(in colliderToCast.m_triMesh, in castStart, castEnd, in target.m_convex, in targetTransform, out result); + return ConvexTriMesh.ColliderCast(in colliderToCast.m_triMesh(), in castStart, castEnd, in target.m_convex, in targetTransform, out result); case (ColliderType.TriMesh, ColliderType.TriMesh): - return TriMeshTriMesh.ColliderCast(in colliderToCast.m_triMesh, in castStart, castEnd, in target.m_triMesh, in targetTransform, out result); + return TriMeshTriMesh.ColliderCast(in colliderToCast.m_triMesh(), in castStart, castEnd, in target.m_triMesh(), in targetTransform, out result); case (ColliderType.TriMesh, ColliderType.Compound): - return TriMeshCompound.ColliderCast(in colliderToCast.m_triMesh, in castStart, castEnd, in target.m_compound, in targetTransform, out result); + return TriMeshCompound.ColliderCast(in colliderToCast.m_triMesh(), in castStart, castEnd, in target.m_compound(), in targetTransform, out result); case (ColliderType.Compound, ColliderType.Sphere): - return SphereCompound.ColliderCast(in colliderToCast.m_compound, in castStart, castEnd, in target.m_sphere, in targetTransform, out result); + return SphereCompound.ColliderCast(in colliderToCast.m_compound(), in castStart, castEnd, in target.m_sphere, in targetTransform, out result); case (ColliderType.Compound, ColliderType.Capsule): - return CapsuleCompound.ColliderCast(in colliderToCast.m_compound, in castStart, castEnd, in target.m_capsule, in targetTransform, out result); + return CapsuleCompound.ColliderCast(in colliderToCast.m_compound(), in castStart, castEnd, in target.m_capsule, in targetTransform, out result); case (ColliderType.Compound, ColliderType.Box): - return BoxCompound.ColliderCast(in colliderToCast.m_compound, in castStart, castEnd, in target.m_box, in targetTransform, out result); + return BoxCompound.ColliderCast(in colliderToCast.m_compound(), in castStart, castEnd, in target.m_box, in targetTransform, out result); case (ColliderType.Compound, ColliderType.Triangle): - return TriangleCompound.ColliderCast(in colliderToCast.m_compound, in castStart, castEnd, in target.m_triangle, in targetTransform, out result); + return TriangleCompound.ColliderCast(in colliderToCast.m_compound(), in castStart, castEnd, in target.m_triangle, in targetTransform, out result); case (ColliderType.Compound, ColliderType.Convex): - return ConvexCompound.ColliderCast(in colliderToCast.m_compound, in castStart, castEnd, in target.m_convex, in targetTransform, out result); + return ConvexCompound.ColliderCast(in colliderToCast.m_compound(), in castStart, castEnd, in target.m_convex, in targetTransform, out result); case (ColliderType.Compound, ColliderType.TriMesh): - return TriMeshCompound.ColliderCast(in colliderToCast.m_compound, in castStart, castEnd, in target.m_triMesh, in targetTransform, out result); + return TriMeshCompound.ColliderCast(in colliderToCast.m_compound(), in castStart, castEnd, in target.m_triMesh(), in targetTransform, out result); case (ColliderType.Compound, ColliderType.Compound): - return CompoundCompound.ColliderCast(in colliderToCast.m_compound, in castStart, castEnd, in target.m_compound, in targetTransform, out result); + return CompoundCompound.ColliderCast(in colliderToCast.m_compound(), in castStart, castEnd, in target.m_compound(), in targetTransform, out result); default: result = default; return false; @@ -585,37 +604,37 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in Collider co return SphereSphere.UnityContactsBetween(in colliderA.m_sphere, in aTransform, in colliderB.m_sphere, in bTransform, in distanceResult); case (ColliderType.Sphere, ColliderType.Capsule): { - var result = SphereCapsule.UnityContactsBetween(in colliderB.m_capsule, in bTransform, in colliderA.m_sphere, in aTransform, in distanceResult); + var result = SphereCapsule.UnityContactsBetween(in colliderB.m_capsule, in bTransform, in colliderA.m_sphere, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Sphere, ColliderType.Box): { - var result = SphereBox.UnityContactsBetween(in colliderB.m_box, in bTransform, in colliderA.m_sphere, in aTransform, in distanceResult); + var result = SphereBox.UnityContactsBetween(in colliderB.m_box, in bTransform, in colliderA.m_sphere, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Sphere, ColliderType.Triangle): { - var result = SphereTriangle.UnityContactsBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_sphere, in aTransform, in distanceResult); + var result = SphereTriangle.UnityContactsBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_sphere, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Sphere, ColliderType.Convex): { - var result = SphereConvex.UnityContactsBetween(in colliderB.m_convex, in bTransform, in colliderA.m_sphere, in aTransform, in distanceResult); + var result = SphereConvex.UnityContactsBetween(in colliderB.m_convex, in bTransform, in colliderA.m_sphere, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Sphere, ColliderType.TriMesh): { - var result = SphereTriMesh.UnityContactsBetween(in colliderB.m_triMesh, in bTransform, in colliderA.m_sphere, in aTransform, in distanceResult); + var result = SphereTriMesh.UnityContactsBetween(in colliderB.m_triMesh(), in bTransform, in colliderA.m_sphere, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Sphere, ColliderType.Compound): { - var result = SphereCompound.UnityContactsBetween(in colliderB.m_compound, in bTransform, in colliderA.m_sphere, in aTransform, in distanceResult); + var result = SphereCompound.UnityContactsBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_sphere, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } @@ -625,31 +644,31 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in Collider co return CapsuleCapsule.UnityContactsBetween(in colliderA.m_capsule, in aTransform, in colliderB.m_capsule, in bTransform, in distanceResult); case (ColliderType.Capsule, ColliderType.Box): { - var result = CapsuleBox.UnityContactsBetween(in colliderB.m_box, in bTransform, in colliderA.m_capsule, in aTransform, in distanceResult); + var result = CapsuleBox.UnityContactsBetween(in colliderB.m_box, in bTransform, in colliderA.m_capsule, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Capsule, ColliderType.Triangle): { - var result = CapsuleTriangle.UnityContactsBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_capsule, in aTransform, in distanceResult); + var result = CapsuleTriangle.UnityContactsBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_capsule, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Capsule, ColliderType.Convex): { - var result = CapsuleConvex.UnityContactsBetween(in colliderB.m_convex, in bTransform, in colliderA.m_capsule, in aTransform, in distanceResult); + var result = CapsuleConvex.UnityContactsBetween(in colliderB.m_convex, in bTransform, in colliderA.m_capsule, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Capsule, ColliderType.TriMesh): { - var result = CapsuleTriMesh.UnityContactsBetween(in colliderB.m_triMesh, in bTransform, in colliderA.m_capsule, in aTransform, in distanceResult); + var result = CapsuleTriMesh.UnityContactsBetween(in colliderB.m_triMesh(), in bTransform, in colliderA.m_capsule, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Capsule, ColliderType.Compound): { - var result = CapsuleCompound.UnityContactsBetween(in colliderB.m_compound, in bTransform, in colliderA.m_capsule, in aTransform, in distanceResult); + var result = CapsuleCompound.UnityContactsBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_capsule, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } @@ -661,25 +680,25 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in Collider co return BoxBox.UnityContactsBetween(in colliderA.m_box, in aTransform, in colliderB.m_box, in bTransform, in distanceResult); case (ColliderType.Box, ColliderType.Triangle): { - var result = BoxTriangle.UnityContactsBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_box, in aTransform, in distanceResult); + var result = BoxTriangle.UnityContactsBetween(in colliderB.m_triangle, in bTransform, in colliderA.m_box, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Box, ColliderType.Convex): { - var result = BoxConvex.UnityContactsBetween(in colliderB.m_convex, in bTransform, in colliderA.m_box, in aTransform, in distanceResult); + var result = BoxConvex.UnityContactsBetween(in colliderB.m_convex, in bTransform, in colliderA.m_box, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Box, ColliderType.TriMesh): { - var result = BoxTriMesh.UnityContactsBetween(in colliderB.m_triMesh, in bTransform, in colliderA.m_box, in aTransform, in distanceResult); + var result = BoxTriMesh.UnityContactsBetween(in colliderB.m_triMesh(), in bTransform, in colliderA.m_box, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Box, ColliderType.Compound): { - var result = BoxCompound.UnityContactsBetween(in colliderB.m_compound, in bTransform, in colliderA.m_box, in aTransform, in distanceResult); + var result = BoxCompound.UnityContactsBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_box, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } @@ -693,19 +712,19 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in Collider co return TriangleTriangle.UnityContactsBetween(in colliderA.m_triangle, in aTransform, in colliderB.m_triangle, in bTransform, in distanceResult); case (ColliderType.Triangle, ColliderType.Convex): { - var result = TriangleConvex.UnityContactsBetween(in colliderB.m_convex, in bTransform, in colliderA.m_triangle, in aTransform, in distanceResult); + var result = TriangleConvex.UnityContactsBetween(in colliderB.m_convex, in bTransform, in colliderA.m_triangle, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Triangle, ColliderType.TriMesh): { - var result = TriangleTriMesh.UnityContactsBetween(in colliderB.m_triMesh, in bTransform, in colliderA.m_triangle, in aTransform, in distanceResult); + var result = TriangleTriMesh.UnityContactsBetween(in colliderB.m_triMesh(), in bTransform, in colliderA.m_triangle, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Triangle, ColliderType.Compound): { - var result = TriangleCompound.UnityContactsBetween(in colliderB.m_compound, in bTransform, in colliderA.m_triangle, in aTransform, in distanceResult); + var result = TriangleCompound.UnityContactsBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_triangle, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } @@ -721,48 +740,48 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in Collider co return ConvexConvex.UnityContactsBetween(in colliderA.m_convex, in aTransform, in colliderB.m_convex, in bTransform, in distanceResult); case (ColliderType.Convex, ColliderType.TriMesh): { - var result = ConvexTriMesh.UnityContactsBetween(in colliderB.m_triMesh, in bTransform, in colliderA.m_convex, in aTransform, in distanceResult); + var result = ConvexTriMesh.UnityContactsBetween(in colliderB.m_triMesh(), in bTransform, in colliderA.m_convex, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Convex, ColliderType.Compound): { - var result = ConvexCompound.UnityContactsBetween(in colliderB.m_compound, in bTransform, in colliderA.m_convex, in aTransform, in distanceResult); + var result = ConvexCompound.UnityContactsBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_convex, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.TriMesh, ColliderType.Sphere): - return SphereTriMesh.UnityContactsBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_sphere, in bTransform, in distanceResult); + return SphereTriMesh.UnityContactsBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_sphere, in bTransform, in distanceResult); case (ColliderType.TriMesh, ColliderType.Capsule): - return CapsuleTriMesh.UnityContactsBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_capsule, in bTransform, in distanceResult); + return CapsuleTriMesh.UnityContactsBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_capsule, in bTransform, in distanceResult); case (ColliderType.TriMesh, ColliderType.Box): - return BoxTriMesh.UnityContactsBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_box, in bTransform, in distanceResult); + return BoxTriMesh.UnityContactsBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_box, in bTransform, in distanceResult); case (ColliderType.TriMesh, ColliderType.Triangle): - return TriangleTriMesh.UnityContactsBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_triangle, in bTransform, in distanceResult); + return TriangleTriMesh.UnityContactsBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_triangle, in bTransform, in distanceResult); case (ColliderType.TriMesh, ColliderType.Convex): - return ConvexTriMesh.UnityContactsBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_convex, in bTransform, in distanceResult); + return ConvexTriMesh.UnityContactsBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_convex, in bTransform, in distanceResult); case (ColliderType.TriMesh, ColliderType.TriMesh): - return TriMeshTriMesh.UnityContactsBetween(in colliderA.m_triMesh, in aTransform, in colliderB.m_triMesh, in bTransform, in distanceResult); + return TriMeshTriMesh.UnityContactsBetween(in colliderA.m_triMesh(), in aTransform, in colliderB.m_triMesh(), in bTransform, in distanceResult); case (ColliderType.TriMesh, ColliderType.Compound): { - var result = TriMeshCompound.UnityContactsBetween(in colliderB.m_compound, in bTransform, in colliderA.m_triMesh, in aTransform, in distanceResult); + var result = TriMeshCompound.UnityContactsBetween(in colliderB.m_compound(), in bTransform, in colliderA.m_triMesh(), in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } case (ColliderType.Compound, ColliderType.Sphere): - return SphereCompound.UnityContactsBetween(in colliderA.m_compound, in aTransform, in colliderB.m_sphere, in bTransform, in distanceResult); + return SphereCompound.UnityContactsBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_sphere, in bTransform, in distanceResult); case (ColliderType.Compound, ColliderType.Capsule): - return CapsuleCompound.UnityContactsBetween(in colliderA.m_compound, in aTransform, in colliderB.m_capsule, in bTransform, in distanceResult); + return CapsuleCompound.UnityContactsBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_capsule, in bTransform, in distanceResult); case (ColliderType.Compound, ColliderType.Box): - return BoxCompound.UnityContactsBetween(in colliderA.m_compound, in aTransform, in colliderB.m_box, in bTransform, in distanceResult); + return BoxCompound.UnityContactsBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_box, in bTransform, in distanceResult); case (ColliderType.Compound, ColliderType.Triangle): - return TriangleCompound.UnityContactsBetween(in colliderA.m_compound, in aTransform, in colliderB.m_triangle, in bTransform, in distanceResult); + return TriangleCompound.UnityContactsBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_triangle, in bTransform, in distanceResult); case (ColliderType.Compound, ColliderType.Convex): - return ConvexCompound.UnityContactsBetween(in colliderA.m_compound, in aTransform, in colliderB.m_convex, in bTransform, in distanceResult); + return ConvexCompound.UnityContactsBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_convex, in bTransform, in distanceResult); case (ColliderType.Compound, ColliderType.TriMesh): - return TriMeshCompound.UnityContactsBetween(in colliderA.m_compound, in aTransform, in colliderB.m_triMesh, in bTransform, in distanceResult); + return TriMeshCompound.UnityContactsBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_triMesh(), in bTransform, in distanceResult); case (ColliderType.Compound, ColliderType.Compound): - return CompoundCompound.UnityContactsBetween(in colliderA.m_compound, in aTransform, in colliderB.m_compound, in bTransform, in distanceResult); + return CompoundCompound.UnityContactsBetween(in colliderA.m_compound(), in aTransform, in colliderB.m_compound(), in bTransform, in distanceResult); default: return default; } diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ColliderColliderDispatch.tt b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ColliderColliderDispatch.tt index 7657a20..95aefc5 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ColliderColliderDispatch.tt +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ColliderColliderDispatch.tt @@ -27,6 +27,17 @@ Dictionary colliderLowerNames = new Dictionary() {"Compound" , "compound" } }; +Dictionary colliderPropertyNames = new Dictionary() +{ + {"Sphere" , "sphere" }, + {"Capsule" , "capsule" }, + {"Box" , "box" }, + {"Triangle" , "triangle" }, + {"Convex" , "convex" }, + {"TriMesh" , "triMesh()" }, + {"Compound" , "compound()" } +}; + int firstComposite = 5; #> using Unity.Burst; @@ -50,11 +61,11 @@ namespace Latios.Psyshock int i = 0; foreach (var colliderTypeNameA in colliderTypeNames) { - var colliderNameA = colliderLowerNames[colliderTypeNameA]; + var colliderNameA = colliderPropertyNames[colliderTypeNameA]; int j = 0; foreach (var colliderTypeNameB in colliderTypeNames) { - var colliderNameB = colliderLowerNames[colliderTypeNameB]; + var colliderNameB = colliderPropertyNames[colliderTypeNameB]; #> case (ColliderType.<#= colliderTypeNameA #>, ColliderType.<#= colliderTypeNameB #>): <# @@ -111,11 +122,11 @@ foreach (var colliderTypeNameA in colliderTypeNames) i = 0; foreach (var colliderTypeNameA in colliderTypeNames) { - var colliderNameA = colliderLowerNames[colliderTypeNameA]; + var colliderNameA = colliderPropertyNames[colliderTypeNameA]; int j = 0; foreach (var colliderTypeNameB in colliderTypeNames) { - var colliderNameB = colliderLowerNames[colliderTypeNameB]; + var colliderNameB = colliderPropertyNames[colliderTypeNameB]; #> case (ColliderType.<#= colliderTypeNameA #>, ColliderType.<#= colliderTypeNameB #>): <# @@ -123,9 +134,11 @@ foreach (var colliderTypeNameA in colliderTypeNames) { #> { - <#= colliderTypeNameA #><#= colliderTypeNameB #>.DistanceBetween(in colliderB.m_<#= colliderNameB #>, in bTransform, in colliderA.m_<#= colliderNameA #>, in aTransform, maxDistance, out result); - result.FlipInPlace(); - processor.Execute(in result); + if (<#= colliderTypeNameA #><#= colliderTypeNameB #>.DistanceBetween(in colliderB.m_<#= colliderNameB #>, in bTransform, in colliderA.m_<#= colliderNameA #>, in aTransform, maxDistance, out result)) + { + result.FlipInPlace(); + processor.Execute(in result); + } break; } <# @@ -134,8 +147,8 @@ foreach (var colliderTypeNameA in colliderTypeNames) { #> { - <#= colliderTypeNameB #><#= colliderTypeNameA #>.DistanceBetween(in colliderA.m_<#= colliderNameA #>, in aTransform, in colliderB.m_<#= colliderNameB #>, in bTransform, maxDistance, out result); - processor.Execute(in result); + if (<#= colliderTypeNameB #><#= colliderTypeNameA #>.DistanceBetween(in colliderA.m_<#= colliderNameA #>, in aTransform, in colliderB.m_<#= colliderNameB #>, in bTransform, maxDistance, out result)) + processor.Execute(in result); break; } <# @@ -175,11 +188,11 @@ foreach (var colliderTypeNameA in colliderTypeNames) i = 0; foreach (var colliderTypeNameA in colliderTypeNames) { - var colliderNameA = colliderLowerNames[colliderTypeNameA]; + var colliderNameA = colliderPropertyNames[colliderTypeNameA]; int j = 0; foreach (var colliderTypeNameB in colliderTypeNames) { - var colliderNameB = colliderLowerNames[colliderTypeNameB]; + var colliderNameB = colliderPropertyNames[colliderTypeNameB]; #> case (ColliderType.<#= colliderTypeNameA #>, ColliderType.<#= colliderTypeNameB #>): <# @@ -218,11 +231,11 @@ foreach (var colliderTypeNameA in colliderTypeNames) i = 0; foreach (var colliderTypeNameA in colliderTypeNames) { - var colliderNameA = colliderLowerNames[colliderTypeNameA]; + var colliderNameA = colliderPropertyNames[colliderTypeNameA]; int j = 0; foreach (var colliderTypeNameB in colliderTypeNames) { - var colliderNameB = colliderLowerNames[colliderTypeNameB]; + var colliderNameB = colliderPropertyNames[colliderTypeNameB]; #> case (ColliderType.<#= colliderTypeNameA #>, ColliderType.<#= colliderTypeNameB #>): <# @@ -230,7 +243,7 @@ foreach (var colliderTypeNameA in colliderTypeNames) { #> { - var result = <#= colliderTypeNameA #><#= colliderTypeNameB #>.UnityContactsBetween(in colliderB.m_<#= colliderNameB #>, in bTransform, in colliderA.m_<#= colliderNameA #>, in aTransform, in distanceResult); + var result = <#= colliderTypeNameA #><#= colliderTypeNameB #>.UnityContactsBetween(in colliderB.m_<#= colliderNameB #>, in bTransform, in colliderA.m_<#= colliderNameA #>, in aTransform, distanceResult.ToFlipped()); result.FlipInPlace(); return result; } diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CompoundCompound.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CompoundCompound.cs index c2e7b01..0656ea7 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CompoundCompound.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/CompoundCompound.cs @@ -149,17 +149,11 @@ private static bool DistanceBetween(in Collider colliderA, return SphereSphere.DistanceBetween(in colliderA.m_sphere, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); case (ColliderType.Sphere, ColliderType.Capsule): var sphereCapsuleResult = SphereCapsule.DistanceBetween(in colliderB.m_capsule, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result); - - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return sphereCapsuleResult; case (ColliderType.Sphere, ColliderType.Box): var sphereBoxResult = SphereBox.DistanceBetween(in colliderB.m_box, in bTransform, in colliderA.m_sphere, in aTransform, maxDistance, out result); - - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return sphereBoxResult; case (ColliderType.Capsule, ColliderType.Sphere): return SphereCapsule.DistanceBetween(in colliderA.m_capsule, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); @@ -167,10 +161,7 @@ private static bool DistanceBetween(in Collider colliderA, return CapsuleCapsule.DistanceBetween(in colliderA.m_capsule, in aTransform, in colliderB.m_capsule, in bTransform, maxDistance, out result); case (ColliderType.Capsule, ColliderType.Box): var capsuleBoxResult = CapsuleBox.DistanceBetween(in colliderB.m_box, in bTransform, in colliderA.m_capsule, in aTransform, maxDistance, out result); - - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return capsuleBoxResult; case (ColliderType.Box, ColliderType.Sphere): return SphereBox.DistanceBetween(in colliderA.m_box, in aTransform, in colliderB.m_sphere, in bTransform, maxDistance, out result); diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexCompound.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexCompound.cs index 110c5c2..d711940 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexCompound.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexCompound.cs @@ -159,9 +159,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return sphereResult; case ColliderType.Capsule: var capsuleResult = CapsuleConvex.DistanceBetween(in convex, @@ -170,9 +168,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return capsuleResult; case ColliderType.Box: var boxResult = BoxConvex.DistanceBetween(in convex, @@ -181,9 +177,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return boxResult; default: result = default; diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexConvex.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexConvex.cs index bc26365..5dd14b6 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexConvex.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexConvex.cs @@ -16,7 +16,7 @@ public static bool DistanceBetween(in ConvexCollider convexA, { var bInATransform = math.mul(math.inverse(aTransform), bTransform); var gjkResult = GjkEpa.DoGjkEpa(convexA, convexB, in bInATransform); - var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(1e-4f, -1e-4f, gjkResult.distance < 0f); + var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(-1e-4f, 1e-4f, gjkResult.distance < 0f); SphereConvex.DistanceBetween(in convexA, in RigidTransform.identity, new SphereCollider(gjkResult.hitpointOnAInASpace + epsilon, 0f), @@ -384,6 +384,7 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in ConvexColli bool needsClosestPoint = true; UnityContactManifoldExtra result = default; + result.baseStorage.contactNormal = contactNormal; if (math.abs(math.dot(facePlaneB.normal, aLocalContactNormal)) > 0.05f) { var distanceScalarAlongContactNormalB = math.rcp(math.dot(aLocalContactNormal, facePlaneB.normal)); @@ -442,7 +443,7 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in ConvexColli for (int i = 0; i < bStackCount; i++) { var edgePlane = edgePlanesA[i]; - projectsOnA &= simd.dot(edgePlane.normals, vertex) + edgePlane.distancesFromOrigin <= 0f; + projectsOnA &= simd.dot(edgePlane.normals, vertex) < edgePlane.distancesFromOrigin; } if (math.all(projectsOnA)) { diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexTriMesh.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexTriMesh.cs index fba3f42..4b863cf 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexTriMesh.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/ConvexTriMesh.cs @@ -14,14 +14,17 @@ public static bool DistanceBetween(in TriMeshCollider triMesh, float maxDistance, out ColliderDistanceResult result) { - var convexInTriMeshTransform = math.mul(math.inverse(triMeshTransform), convexTransform); - var aabb = Physics.AabbFrom(convex, in convexInTriMeshTransform); - var processor = new ConvexDistanceProcessor + var convexInTriMeshTransform = math.mul(math.inverse(triMeshTransform), convexTransform); + var aabb = Physics.AabbFrom(convex, in convexInTriMeshTransform); + aabb.min -= maxDistance; + aabb.max += maxDistance; + var processor = new ConvexDistanceProcessor { blob = triMesh.triMeshColliderBlob, convex = convex, convexTransform = convexInTriMeshTransform, maxDistance = maxDistance, + bestDistance = float.MaxValue, found = false, scale = triMesh.scale }; @@ -52,9 +55,11 @@ public static unsafe void DistanceBetweenAll(in TriMeshCollider triMesh, float maxDistance, ref T processor) where T : unmanaged, IDistanceBetweenAllProcessor { - var convexInTriMeshTransform = math.mul(math.inverse(triMeshTransform), convexTransform); - var aabb = Physics.AabbFrom(convex, convexInTriMeshTransform); - var triProcessor = new DistanceAllProcessor + var convexInTriMeshTransform = math.mul(math.inverse(triMeshTransform), convexTransform); + var aabb = Physics.AabbFrom(convex, convexInTriMeshTransform); + aabb.min -= maxDistance; + aabb.max += maxDistance; + var triProcessor = new DistanceAllProcessor { triMesh = triMesh, triMeshTransform = triMeshTransform, diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/SphereTriMesh.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/SphereTriMesh.cs index 9df2a20..0661b9e 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/SphereTriMesh.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/SphereTriMesh.cs @@ -19,10 +19,11 @@ public static bool DistanceBetween(in TriMeshCollider triMesh, var aabb = Physics.AabbFrom(pointInTriMesh - maxDistance, pointInTriMesh + maxDistance); var processor = new PointRayTriMesh.PointProcessor { - blob = triMesh.triMeshColliderBlob, - point = pointInTriMesh, - maxDistance = maxDistance + sphere.radius, - found = false + blob = triMesh.triMeshColliderBlob, + point = pointInTriMesh, + maxDistance = maxDistance + sphere.radius, + bestDistance = float.MaxValue, + found = false }; triMesh.triMeshColliderBlob.Value.FindTriangles(in aabb, ref processor); if (processor.found) @@ -43,9 +44,11 @@ public static unsafe void DistanceBetweenAll(in TriMeshCollider triMesh, float maxDistance, ref T processor) where T : unmanaged, IDistanceBetweenAllProcessor { - var sphereInTriMeshTransform = math.mul(math.inverse(triMeshTransform), sphereTransform); - var aabb = Physics.AabbFrom(sphere, sphereInTriMeshTransform); - var triProcessor = new DistanceAllProcessor + var sphereInTriMeshTransform = math.mul(math.inverse(triMeshTransform), sphereTransform); + var aabb = Physics.AabbFrom(sphere, sphereInTriMeshTransform); + aabb.min -= maxDistance; + aabb.max += maxDistance; + var triProcessor = new DistanceAllProcessor { triMesh = triMesh, triMeshTransform = triMeshTransform, diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriMeshCompound.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriMeshCompound.cs index b5399da..f66f485 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriMeshCompound.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriMeshCompound.cs @@ -158,9 +158,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return sphereResult; case ColliderType.Capsule: var capsuleResult = CapsuleTriMesh.DistanceBetween(in triMesh, @@ -169,9 +167,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return capsuleResult; case ColliderType.Box: var boxResult = BoxTriMesh.DistanceBetween(in triMesh, @@ -180,9 +176,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return boxResult; default: result = default; diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriMeshTriMesh.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriMeshTriMesh.cs index 399410b..0551eee 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriMeshTriMesh.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriMeshTriMesh.cs @@ -20,10 +20,10 @@ public static bool DistanceBetween(in TriMeshCollider triMeshA, Physics.TransformAabb(new TransformQvvs(transformAinB.pos, transformAinB.rot, 1f, triMeshA.scale), in triMeshA.triMeshColliderBlob.Value.localAabb); var aabbBinA = Physics.TransformAabb(new TransformQvvs(transformBinA.pos, transformBinA.rot, 1f, triMeshB.scale), in triMeshB.triMeshColliderBlob.Value.localAabb); - aabbAinB.min *= maxDistance; - aabbAinB.max *= maxDistance; - aabbBinA.min *= maxDistance; - aabbBinA.max *= maxDistance; + aabbAinB.min -= maxDistance; + aabbAinB.max += maxDistance; + aabbBinA.min -= maxDistance; + aabbBinA.max += maxDistance; var processor = new TriMeshDistanceOuterProcessor { @@ -31,13 +31,15 @@ public static bool DistanceBetween(in TriMeshCollider triMeshA, { blobA = triMeshA.triMeshColliderBlob, maxDistance = maxDistance, + bestDistance = float.MaxValue, scaleA = triMeshA.scale, transformBinA = transformBinA, }, - aabbBinA = aabbBinA, - blobB = triMeshB.triMeshColliderBlob, - found = false, - scaleB = triMeshB.scale, + aabbBinA = aabbBinA, + blobB = triMeshB.triMeshColliderBlob, + bestDistance = float.MaxValue, + found = false, + scaleB = triMeshB.scale, }; triMeshB.triMeshColliderBlob.Value.FindTriangles(in aabbAinB, ref processor, triMeshB.scale); diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleCompound.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleCompound.cs index 6ccfa0f..d4975cc 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleCompound.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleCompound.cs @@ -158,9 +158,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return sphereResult; case ColliderType.Capsule: var capsuleResult = CapsuleTriangle.DistanceBetween(in triangle, @@ -169,9 +167,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return capsuleResult; case ColliderType.Box: var boxResult = BoxTriangle.DistanceBetween(in triangle, @@ -180,9 +176,7 @@ private static bool DistanceBetween(in Collider collider, in colliderTransform, maxDistance, out result); - (result.hitpointA, result.hitpointB) = (result.hitpointB, result.hitpointA); - (result.normalA, result.normalB) = (result.normalB, result.normalA); - (result.subColliderIndexA, result.subColliderIndexB) = (result.subColliderIndexB, result.subColliderIndexA); + result.FlipInPlace(); return boxResult; default: result = default; diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleConvex.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleConvex.cs index a3f0ef9..1ef6fe4 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleConvex.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleConvex.cs @@ -15,7 +15,7 @@ public static bool DistanceBetween(in ConvexCollider convex, { var bInATransform = math.mul(math.inverse(convexTransform), triangleTransform); var gjkResult = GjkEpa.DoGjkEpa(convex, triangle, in bInATransform); - var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(1e-4f, -1e-4f, gjkResult.distance < 0f); + var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(-1e-4f, 1e-4f, gjkResult.distance < 0f); SphereConvex.DistanceBetween(in convex, in RigidTransform.identity, new SphereCollider(gjkResult.hitpointOnAInASpace + epsilon, 0f), @@ -166,10 +166,11 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in ConvexColli out var bVertices); bool needsClosestPoint = true; UnityContactManifoldExtra3D result = default; + result.baseStorage.contactNormal = contactNormal; if (math.abs(math.dot(bPlane.normal, aLocalContactNormal)) > 0.05f) { - var distanceScalarAlongContactNormalB = math.rcp(math.dot(aLocalContactNormal, bPlane.normal)); + var distanceScalarAlongContactNormalB = math.rcp(math.dot(-aLocalContactNormal, bPlane.normal)); bool projectBOnA = math.abs(math.dot(aPlane.normal, aLocalContactNormal)) < 0.05f; int4 positiveSideCounts = 0; @@ -220,9 +221,9 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in ConvexColli { var aEdgePlaneNormal = math.cross(rayDisplacement, aLocalContactNormal); var edgePlaneDistance = math.dot(aEdgePlaneNormal, rayStart); - var projection = simd.dot(bVertices, aEdgePlaneNormal) + edgePlaneDistance; - positiveSideCounts += math.select(int4.zero, 1, projection > 0f); - negativeSideCounts += math.select(int4.zero, 1, projection < 0f); + var projection = simd.dot(bVertices, aEdgePlaneNormal); + positiveSideCounts += math.select(int4.zero, 1, projection > edgePlaneDistance); + negativeSideCounts += math.select(int4.zero, 1, projection < edgePlaneDistance); } } if (projectBOnA) @@ -469,7 +470,7 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in ConvexColli if (math.abs(math.dot(bPlane.normal, aLocalContactNormal)) > 0.05f) { - var distanceScalarAlongContactNormalB = math.rcp(math.dot(aLocalContactNormal, bPlane.normal)); + var distanceScalarAlongContactNormalB = math.rcp(math.dot(-aLocalContactNormal, bPlane.normal)); bool projectBOnA = math.abs(math.dot(aPlane.normal, aLocalContactNormal)) < 0.05f; int4 positiveSideCounts = 0; @@ -530,9 +531,9 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in ConvexColli { var aEdgePlaneNormal = math.cross(rayDisplacement, aLocalContactNormal); var edgePlaneDistance = math.dot(aEdgePlaneNormal, rayStart); - var projection = simd.dot(bVertices, aEdgePlaneNormal) + edgePlaneDistance; - positiveSideCounts += math.select(int4.zero, 1, projection > 0f); - negativeSideCounts += math.select(int4.zero, 1, projection < 0f); + var projection = simd.dot(bVertices, aEdgePlaneNormal); + positiveSideCounts += math.select(int4.zero, 1, projection > edgePlaneDistance); + negativeSideCounts += math.select(int4.zero, 1, projection < edgePlaneDistance); } } if (projectBOnA) diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleTriMesh.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleTriMesh.cs index 3726841..2dd3cb4 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleTriMesh.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleTriMesh.cs @@ -14,14 +14,17 @@ public static bool DistanceBetween(in TriMeshCollider triMesh, float maxDistance, out ColliderDistanceResult result) { - var triangleInTriMeshTransform = math.mul(math.inverse(triMeshTransform), triangleTransform); - var aabb = Physics.AabbFrom(triangle, in triangleInTriMeshTransform); - var processor = new TriangleDistanceProcessor + var triangleInTriMeshTransform = math.mul(math.inverse(triMeshTransform), triangleTransform); + var aabb = Physics.AabbFrom(triangle, in triangleInTriMeshTransform); + aabb.min -= maxDistance; + aabb.max += maxDistance; + var processor = new TriangleDistanceProcessor { blob = triMesh.triMeshColliderBlob, triangle = triangle, triangleTransform = triangleInTriMeshTransform, maxDistance = maxDistance, + bestDistance = float.MaxValue, found = false, scale = triMesh.scale }; @@ -49,9 +52,11 @@ public static unsafe void DistanceBetweenAll(in TriMeshCollider triMesh, float maxDistance, ref T processor) where T : unmanaged, IDistanceBetweenAllProcessor { - var triangleInTriMeshTransform = math.mul(math.inverse(triMeshTransform), triangleTransform); - var aabb = Physics.AabbFrom(triangle, triangleInTriMeshTransform); - var triProcessor = new DistanceAllProcessor + var triangleInTriMeshTransform = math.mul(math.inverse(triMeshTransform), triangleTransform); + var aabb = Physics.AabbFrom(triangle, triangleInTriMeshTransform); + aabb.min -= maxDistance; + aabb.max += maxDistance; + var triProcessor = new DistanceAllProcessor { triMesh = triMesh, triMeshTransform = triMeshTransform, diff --git a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleTriangle.cs b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleTriangle.cs index ee2c9b2..ed0004f 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleTriangle.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/ColliderCollider/TriangleTriangle.cs @@ -15,7 +15,7 @@ public static bool DistanceBetween(in TriangleCollider triangleA, // Todo: SAT algorithm similar to box vs box. var bInATransform = math.mul(math.inverse(aTransform), bTransform); var gjkResult = GjkEpa.DoGjkEpa(triangleA, triangleB, in bInATransform); - var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(1e-4f, -1e-4f, gjkResult.distance < 0f); + var epsilon = gjkResult.normalizedOriginToClosestCsoPoint * math.select(-1e-4f, 1e-4f, gjkResult.distance < 0f); SphereTriangle.DistanceBetween(in triangleA, in RigidTransform.identity, new SphereCollider(gjkResult.hitpointOnAInASpace + epsilon, 0f), @@ -160,7 +160,7 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in TriangleCol if (math.abs(math.dot(bPlane.normal, aLocalContactNormal)) > 0.05f) { - var distanceScalarAlongContactNormalB = math.rcp(math.dot(aLocalContactNormal, bPlane.normal)); + var distanceScalarAlongContactNormalB = math.rcp(math.dot(-aLocalContactNormal, bPlane.normal)); // Project and clip edges of A onto the face of B. for (int edgeIndex = 0; edgeIndex < 3; edgeIndex++) @@ -202,7 +202,7 @@ public static UnitySim.ContactsBetweenResult UnityContactsBetween(in TriangleCol for (int i = 0; i < 3; i++) { var vertex = bVertices[i]; - if (math.all(simd.dot(aEdgePlaneNormals, vertex) + aEdgePlaneDistances <= 0f)) + if (math.all(simd.dot(aEdgePlaneNormals, vertex) < aEdgePlaneDistances)) { var distance = mathex.SignedDistance(aPlane, vertex) * distanceScalarAlongContactNormalA; result.Add(math.transform(aTransform, vertex), distance); diff --git a/PsyshockPhysics/Physics/Internal/Queries/Generalized/GjkEpa.cs b/PsyshockPhysics/Physics/Internal/Queries/Generalized/GjkEpa.cs index eef5dd9..79c623b 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/Generalized/GjkEpa.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/Generalized/GjkEpa.cs @@ -249,7 +249,7 @@ public static unsafe GjkResult DoGjkEpa(in Collider colliderA, in Collider colli if (hull.triangles[triangleIndex].uid != uidsCache[triangleIndex]) { uidsCache[triangleIndex] = hull.triangles[triangleIndex].uid; - distancesCache[triangleIndex] = hull.ComputePlane(triangleIndex).distanceFromOrigin; + distancesCache[triangleIndex] = hull.ComputePlane(triangleIndex).distanceToOrigin; } if (closestTriangleIndex == -1 || distancesCache[closestTriangleIndex] < distancesCache[triangleIndex]) { @@ -260,7 +260,7 @@ public static unsafe GjkResult DoGjkEpa(in Collider colliderA, in Collider colli // Add supporting vertex or exit. var sv = MinkowskiSupports.GetSupport(colliderA, colliderB, closestPlane.normal, bInASpace); - float d2P = math.dot(closestPlane.normal, sv.pos) + closestPlane.distanceFromOrigin; + float d2P = math.dot(closestPlane.normal, sv.pos) + closestPlane.distanceToOrigin; if (math.abs(d2P) > stopThreshold && hull.AddPoint(sv.pos, sv.id)) stopThreshold *= 1.3f; else @@ -271,7 +271,7 @@ public static unsafe GjkResult DoGjkEpa(in Collider colliderA, in Collider colli // There could be multiple triangles in the closest plane, pick the one that has the closest point to the origin on its face foreach (int triangleIndex in hull.triangles.indices) { - if (distancesCache[triangleIndex] >= closestPlane.distanceFromOrigin - k_epaEpsilon) + if (distancesCache[triangleIndex] >= closestPlane.distanceToOrigin - k_epaEpsilon) { ConvexHullBuilder.Triangle triangle = hull.triangles[triangleIndex]; float3 a = hull.vertices[triangle.vertex0].position; @@ -303,11 +303,11 @@ public static unsafe GjkResult DoGjkEpa(in Collider colliderA, in Collider colli simplex.b.pos = hull.vertices[triangle.vertex1].position; simplex.b.id = hull.vertices[triangle.vertex1].userData; simplex.c.pos = hull.vertices[triangle.vertex2].position; simplex.c.id = hull.vertices[triangle.vertex2].userData; simplex.originToClosestPointUnscaledDirection = -closestPlane.normal; - simplex.scaledDistance = closestPlane.distanceFromOrigin; + simplex.scaledDistance = closestPlane.distanceToOrigin; // Set normal and distance. normalizedOriginToClosestCsoPoint = -closestPlane.normal; - result.distance = closestPlane.distanceFromOrigin; + result.distance = closestPlane.distanceToOrigin; } } } diff --git a/PsyshockPhysics/Physics/Internal/Queries/Generalized/Mpr.cs b/PsyshockPhysics/Physics/Internal/Queries/Generalized/Mpr.cs index fcb07fd..cbd8bb1 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/Generalized/Mpr.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/Generalized/Mpr.cs @@ -437,7 +437,7 @@ private static float DoMprRefine3D(in Collider colliderA, fractions = math.select(float.MaxValue, fractions, hit); return (1f - math.cmin(fractions)) * rayLength; } - return math.abs(plane.distanceFromOrigin / denom); + return math.abs(plane.distanceToOrigin / denom); } private static bool DoPlanarMpr(in Collider colliderA, @@ -782,7 +782,7 @@ private static float DoMprRefine3DDebug(in Collider colliderA, // We don't use triangle raycast here because precision issues could cause our ray to miss. Plane plane = mathex.PlaneFrom(portalA.pos, portalB.pos - portalA.pos, portalC.pos - portalA.pos); float denom = math.dot(plane.normal, normalizedSearchDirectionInASpace); - UnityEngine.Debug.Log($"plane: {plane.normal}, {plane.distanceFromOrigin}, denom: {denom}"); + UnityEngine.Debug.Log($"plane: {plane.normal}, {plane.distanceToOrigin}, denom: {denom}"); if (math.abs(denom) < math.EPSILON) { // The triangle is coplanar with the ray. @@ -853,7 +853,7 @@ private static float DoMprRefine3DDebug(in Collider colliderA, UnityEngine.Debug.Log("Coplanar portal is triangle. hit: {hit}"); return (1f - math.cmin(fractions)) * rayLength; } - return math.abs(plane.distanceFromOrigin / denom); + return math.abs(plane.distanceToOrigin / denom); } private static bool DoPlanarMprDebug(in Collider colliderA, diff --git a/PsyshockPhysics/Physics/Internal/Queries/InternalQueryTypes.cs b/PsyshockPhysics/Physics/Internal/Queries/InternalQueryTypes.cs index 313c13a..b3ad973 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/InternalQueryTypes.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/InternalQueryTypes.cs @@ -49,6 +49,16 @@ public static ColliderDistanceResult BinAResultToWorld(in ColliderDistanceResult }; } + public static int GetSubcolliders(in Collider collider) + { + switch(collider.type) + { + case ColliderType.TriMesh: return collider.m_triMesh().triMeshColliderBlob.Value.triangles.Length; + case ColliderType.Compound: return collider.m_compound().compoundColliderBlob.Value.blobColliders.Length; + default: return 1; + } + } + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] internal static void CheckMprResolved(bool somethingWentWrong) { diff --git a/PsyshockPhysics/Physics/Internal/Queries/Layers/BurstEarlyInit.cs b/PsyshockPhysics/Physics/Internal/Queries/Layers/BurstEarlyInit.cs index 759f91d..a47914d 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/Layers/BurstEarlyInit.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/Layers/BurstEarlyInit.cs @@ -16,8 +16,9 @@ static void InitEditor() { var pairsTypes = UnityEditor.TypeCache.GetTypesDerivedFrom(); var objectsTypes = UnityEditor.TypeCache.GetTypesDerivedFrom(); + var foreachTypes = UnityEditor.TypeCache.GetTypesDerivedFrom(); - InitProcessors(pairsTypes, objectsTypes); + InitProcessors(pairsTypes, objectsTypes, foreachTypes); } #else [RuntimeInitializeOnLoadMethod(RuntimeInitializeLoadType.AfterAssembliesLoaded)] @@ -25,8 +26,10 @@ static void InitRuntime() { var pairsTypes = new List(); var objectsTypes = new List(); + var foreachTypes = new List(); var pairsType = typeof(IFindPairsProcessor); var objectsType = typeof(IFindObjectsProcessor); + var foreachType = typeof(IForEachPairProcessor); foreach (var assembly in AppDomain.CurrentDomain.GetAssemblies()) { if (!BootstrapTools.IsAssemblyReferencingSubstring(assembly, "Psyshock")) @@ -41,6 +44,8 @@ static void InitRuntime() pairsTypes.Add(t); if (objectsType.IsAssignableFrom(t)) objectsTypes.Add(t); + if (foreachType.IsAssignableFrom(t)) + foreachTypes.Add(t); } } catch (ReflectionTypeLoadException e) @@ -51,17 +56,19 @@ static void InitRuntime() pairsTypes.Add(t); if (t != null && objectsType.IsAssignableFrom(t)) objectsTypes.Add(t); + if (t != null && foreachType.IsAssignableFrom(t)) + foreachTypes.Add(t); } Debug.LogWarning($"Psyshock BurstEarlyInit.cs failed loading assembly: {(assembly.IsDynamic ? assembly.ToString() : assembly.Location)}"); } } - InitProcessors(pairsTypes, objectsTypes); + InitProcessors(pairsTypes, objectsTypes, foreachTypes); } #endif - static void InitProcessors(IEnumerable findPairsTypes, IEnumerable findObjectsTypes) + static void InitProcessors(IEnumerable findPairsTypes, IEnumerable findObjectsTypes, IEnumerable foreachTypes) { RuntimeConstants.InitConstants(); @@ -102,6 +109,25 @@ static void InitProcessors(IEnumerable findPairsTypes, IEnumerable f Debug.LogException(ex); } } + + var foreachIniterType = typeof(ForeachIniter<>); + + foreach (var foreachType in foreachTypes) + { + try + { + if (foreachType.IsGenericType || foreachType.IsInterface) + continue; + + var type = foreachIniterType.MakeGenericType(foreachType); + var initer = Activator.CreateInstance(type) as IIniter; + initer.Init(); + } + catch (Exception ex) + { + Debug.LogException(ex); + } + } } interface IIniter @@ -109,38 +135,28 @@ interface IIniter void Init(); } - struct FindPairsIniter : IIniter where T : struct, IFindPairsProcessor + public struct FindPairsIniter : IIniter where T : struct, IFindPairsProcessor { public void Init() { - IJobExtensions.EarlyJobInit.FindPairsInternal.LayerSelfSingle>(); - IJobParallelForExtensions.EarlyJobInit.FindPairsInternal.LayerSelfPart1>(); - IJobExtensions.EarlyJobInit.FindPairsInternal.LayerSelfPart2>(); - IJobForExtensions.EarlyJobInit.FindPairsInternal.LayerSelfParallelUnsafe>(); - IJobParallelForExtensions.EarlyJobInit.FindPairsInternal.LayerSelfPart1>(); - IJobExtensions.EarlyJobInit.FindPairsInternal.LayerSelfPart2>(); - - IJobExtensions.EarlyJobInit.FindPairsInternal.LayerLayerSingle>(); - IJobParallelForExtensions.EarlyJobInit.FindPairsInternal.LayerLayerPart1>(); - IJobParallelForExtensions.EarlyJobInit.FindPairsInternal.LayerLayerPart2>(); - IJobForExtensions.EarlyJobInit.FindPairsInternal.LayerLayerParallelUnsafe>(); - IJobParallelForExtensions.EarlyJobInit.FindPairsInternal.LayerLayerPart1>(); - IJobParallelForExtensions.EarlyJobInit.FindPairsInternal.LayerLayerPart2>(); - -#if ENABLE_UNITY_COLLECTIONS_CHECKS - IJobForExtensions.EarlyJobInit.FindPairsInternal.LayerSelfPart2_WithSafety>(); - IJobForExtensions.EarlyJobInit.FindPairsInternal.LayerSelfPart2_WithSafety>(); - IJobParallelForExtensions.EarlyJobInit.FindPairsInternal.LayerLayerPart2_WithSafety>(); - IJobParallelForExtensions.EarlyJobInit.FindPairsInternal.LayerLayerPart2_WithSafety>(); -#endif + IJobForExtensions.EarlyJobInit.FindPairsInternal.LayerSelfJob>(); + IJobForExtensions.EarlyJobInit.FindPairsInternal.LayerLayerJob>(); + } + } + + public struct FindObjectsIniter : IIniter where T : struct, IFindObjectsProcessor + { + public void Init() + { + IJobExtensions.EarlyJobInit.FindObjectsInternal.SingleJob>(); } } - struct FindObjectsIniter : IIniter where T : struct, IFindObjectsProcessor + public struct ForeachIniter : IIniter where T : struct, IForEachPairProcessor { public void Init() { - IJobExtensions.EarlyJobInit.FindObjectsInternal.Single>(); + IJobForExtensions.EarlyJobInit.ForEachPairInternal.ForEachPairJob>(); } } } diff --git a/PsyshockPhysics/Physics/Internal/Queries/Layers/FindObjectsInternal.cs b/PsyshockPhysics/Physics/Internal/Queries/Layers/FindObjectsInternal.cs index f05632c..9b8cfdc 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/Layers/FindObjectsInternal.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/Layers/FindObjectsInternal.cs @@ -6,6 +6,7 @@ using Unity.Entities; using Unity.Jobs; using Unity.Mathematics; +using UnityEngine.Scripting; //Todo: Stream types, single schedulers, scratchlists, and inflations namespace Latios.Psyshock @@ -16,7 +17,7 @@ internal static class FindObjectsInternal { #region Jobs [BurstCompile] - public struct Single : IJob + public struct SingleJob : IJob { [ReadOnly] public CollisionLayer layer; public T processor; @@ -26,6 +27,12 @@ public void Execute() { LayerQuerySweepMethods.AabbSweep(in aabb, in layer, ref processor); } + + [Preserve] + void RequireEarlyJobInit() + { + new InitJobsForProcessors.FindObjectsIniter().Init(); + } } #endregion diff --git a/PsyshockPhysics/Physics/Internal/Queries/Layers/FindPairsInternal.cs b/PsyshockPhysics/Physics/Internal/Queries/Layers/FindPairsInternal.cs index a191a59..8a5ccf4 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/Layers/FindPairsInternal.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/Layers/FindPairsInternal.cs @@ -1,1281 +1,900 @@ using System; using System.Diagnostics; +using Latios.Unsafe; using Unity.Burst; using Unity.Burst.CompilerServices; using Unity.Collections; using Unity.Entities; using Unity.Jobs; using Unity.Mathematics; +using Unity.Profiling; +using UnityEngine.Scripting; //Todo: FilteredCache playback and inflations namespace Latios.Psyshock { public partial struct FindPairsLayerSelfConfig where T : struct, IFindPairsProcessor { + internal enum ScheduleMode + { + Single, + ParallelPart1, + ParallelPart1AllowEntityAliasing, + ParallelPart2, + ParallelPart2AllowEntityAliasing, + ParallelUnsafe + } + internal static class FindPairsInternal { - #region Jobs [BurstCompile] - public struct LayerSelfSingle : IJob + public struct LayerSelfJob : IJobFor { - [ReadOnly] public CollisionLayer layer; - public T processor; + [ReadOnly] CollisionLayer layer; + T processor; + ScheduleMode scheduleMode; + Unity.Profiling.ProfilerMarker modeAndTMarker; - public void Execute() + #region Construction and Scheduling + public LayerSelfJob(in CollisionLayer layer, in T processor) { - RunImmediate(layer, ref processor, true); + this.layer = layer; + this.processor = processor; + scheduleMode = default; + modeAndTMarker = default; } - } - [BurstCompile] - public struct LayerSelfPart1 : IJobParallelFor - { - [ReadOnly] public CollisionLayer layer; - public T processor; - - public void Execute(int index) + public void Run() { - var bucket = layer.GetBucketSlices(index); - var context = new FindPairsBucketContext(in layer, in layer, bucket.bucketGlobalStart, bucket.count, bucket.bucketGlobalStart, bucket.count, index, true); - processor.BeginBucket(in context); - FindPairsSweepMethods.SelfSweep(layer, bucket, index, ref processor); - processor.EndBucket(in context); + SetScheduleMode(ScheduleMode.Single); + this.Run(1); } - } - - [BurstCompile] - public struct LayerSelfPart2 : IJob - { - [ReadOnly] public CollisionLayer layer; - public T processor; - public void Execute() + public JobHandle ScheduleSingle(JobHandle inputDeps) { - var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); - for (int i = 0; i < layer.bucketCount - 1; i++) - { - var bucket = layer.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layer, - in layer, - bucket.bucketGlobalStart, - bucket.count, - crossBucket.bucketGlobalStart, - crossBucket.count, - layer.bucketCount + i, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layer, layer, bucket, crossBucket, layer.bucketCount + i, ref processor); - processor.EndBucket(in context); - } + SetScheduleMode(ScheduleMode.Single); + return this.Schedule(1, inputDeps); } - } - - [BurstCompile] - public struct LayerSelfParallelUnsafe : IJobFor - { - [ReadOnly] public CollisionLayer layer; - public T processor; - public void Execute(int i) + public JobHandle ScheduleParallel(JobHandle inputDeps, ScheduleMode scheduleMode) { - if (i < layer.bucketCount) - { - var bucket = layer.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layer, in layer, bucket.bucketGlobalStart, bucket.count, bucket.bucketGlobalStart, bucket.count, i, true); - processor.BeginBucket(in context); - FindPairsSweepMethods.SelfSweep(layer, bucket, i, ref processor); - processor.EndBucket(in context); + SetScheduleMode(scheduleMode); + if (scheduleMode == ScheduleMode.ParallelPart1 || scheduleMode == ScheduleMode.ParallelPart1AllowEntityAliasing) + return this.ScheduleParallel(layer.bucketCount, 1, inputDeps); + if (scheduleMode == ScheduleMode.ParallelPart2) +#if ENABLE_UNITY_COLLECTIONS_CHECKS + return this.ScheduleParallel(2, 1, inputDeps); +#else + return this.ScheduleParallel(1, 1, inputDeps); +#endif + if (scheduleMode == ScheduleMode.ParallelPart2AllowEntityAliasing) + return this.ScheduleParallel(1, 1, inputDeps); + if (scheduleMode == ScheduleMode.ParallelUnsafe) + return this.ScheduleParallel(layer.bucketCount * 2 - 1, 1, inputDeps); + return inputDeps; + } + + void SetScheduleMode(ScheduleMode scheduleMode) + { + this.scheduleMode = scheduleMode; + FixedString32Bytes modeString = default; + if (scheduleMode == ScheduleMode.Single) + modeString = "Single"; + else if (scheduleMode == ScheduleMode.ParallelPart1) + modeString = "ParallelPart1"; + else if (scheduleMode == ScheduleMode.ParallelPart1AllowEntityAliasing) + modeString = "ParallelPart1_EntityAliasing"; + else if (scheduleMode == ScheduleMode.ParallelPart2) + modeString = "ParallelPart2"; + else if (scheduleMode == ScheduleMode.ParallelPart2AllowEntityAliasing) + modeString = "ParallelPart2_EntityAliasing"; + else if (scheduleMode == ScheduleMode.ParallelUnsafe) + modeString = "ParallelUnsafe"; + + bool isBurst = true; + IsBurst(ref isBurst); + if (isBurst) + { + modeAndTMarker = new Unity.Profiling.ProfilerMarker($"{modeString}_{processor}"); } else { - i -= layer.bucketCount; - var bucket = layer.GetBucketSlices(i); - var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); - var context = new FindPairsBucketContext(in layer, - in layer, - bucket.bucketGlobalStart, - bucket.count, - crossBucket.bucketGlobalStart, - crossBucket.count, - layer.bucketCount + i, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layer, layer, bucket, crossBucket, i + layer.bucketCount, ref processor); - processor.EndBucket(in context); + FixedString128Bytes processorName = default; + GetProcessorNameNoBurst(ref processorName); + modeAndTMarker = new Unity.Profiling.ProfilerMarker($"{modeString}_{processorName}"); } } - } - #endregion - #region ImmediateMethods - public static void RunImmediate(in CollisionLayer layer, ref T processor, bool isThreadSafe) - { - int jobIndex = 0; - for (int i = 0; i < layer.bucketCount; i++) - { - var bucket = layer.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layer, - in layer, - bucket.bucketGlobalStart, - bucket.count, - bucket.bucketGlobalStart, - bucket.count, - jobIndex, - isThreadSafe); - processor.BeginBucket(in context); - FindPairsSweepMethods.SelfSweep(layer, bucket, jobIndex, ref processor, isThreadSafe); - processor.EndBucket(in context); - jobIndex++; - } + [BurstDiscard] + static void IsBurst(ref bool isBurst) => isBurst = false; - var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); - for (int i = 0; i < layer.bucketCount - 1; i++) + [BurstDiscard] + static void GetProcessorNameNoBurst(ref FixedString128Bytes name) { - var bucket = layer.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layer, - in layer, - bucket.bucketGlobalStart, - bucket.count, - crossBucket.bucketGlobalStart, - crossBucket.count, - jobIndex, - isThreadSafe); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layer, layer, bucket, crossBucket, jobIndex, ref processor, isThreadSafe); - processor.EndBucket(in context); - jobIndex++; + name = nameof(T); } - } - #endregion - - #region SafeChecks - -#if ENABLE_UNITY_COLLECTIONS_CHECKS - // Scheudle for 2 iterations - [BurstCompile] - public struct LayerSelfPart2_WithSafety : IJobFor - { - [ReadOnly] public CollisionLayer layer; - public T processor; + #endregion + #region Job Processing public void Execute(int index) { - if (index == 0) + using var jobName = modeAndTMarker.Auto(); + if (scheduleMode == ScheduleMode.Single) + { + RunImmediate(in layer, ref processor, true); + return; + } + if (scheduleMode == ScheduleMode.ParallelPart1 || scheduleMode == ScheduleMode.ParallelPart1AllowEntityAliasing) + { + bool isThreadSafe = scheduleMode == ScheduleMode.ParallelPart1; + Physics.kCellMarker.Begin(); + var bucket = layer.GetBucketSlices(index); + var context = new FindPairsBucketContext(in layer, + in layer, + in bucket, + in bucket, + index, + isThreadSafe, + isThreadSafe); + if (processor.BeginBucket(in context)) + { + if (index != layer.bucketCount - 1) + FindPairsSweepMethods.SelfSweepCell(in layer, in bucket, index, ref processor, isThreadSafe, isThreadSafe); + else + FindPairsSweepMethods.SelfSweepCross(in layer, in bucket, index, ref processor, isThreadSafe, isThreadSafe); + processor.EndBucket(in context); + } + Physics.kCellMarker.End(); + return; + } + if (scheduleMode == ScheduleMode.ParallelPart2 || scheduleMode == ScheduleMode.ParallelPart2AllowEntityAliasing) { + if (index == 1) + { + EntityAliasCheck(in layer); + return; + } + + bool isThreadSafe = scheduleMode == ScheduleMode.ParallelPart2; + Physics.kCrossMarker.Begin(); var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); for (int i = 0; i < layer.bucketCount - 1; i++) { var bucket = layer.GetBucketSlices(i); var context = new FindPairsBucketContext(in layer, in layer, - bucket.bucketGlobalStart, - bucket.count, - crossBucket.bucketGlobalStart, - crossBucket.count, + in bucket, + in crossBucket, layer.bucketCount + i, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layer, layer, bucket, crossBucket, layer.bucketCount + i, ref processor); - processor.EndBucket(in context); + isThreadSafe, + isThreadSafe); + if (processor.BeginBucket(in context)) + { + FindPairsSweepMethods.BipartiteSweepCellCross(layer, layer, bucket, crossBucket, layer.bucketCount + i, ref processor, isThreadSafe, isThreadSafe); + processor.EndBucket(in context); + } } + Physics.kCrossMarker.End(); + return; } - else - { - EntityAliasCheck(layer); - } - } - } - - [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] - private static void EntityAliasCheck(CollisionLayer layer) - { - var hashSet = new NativeParallelHashSet(layer.count, Allocator.Temp); - for (int i = 0; i < layer.count; i++) - { - if (!hashSet.Add(layer.bodies[i].entity)) + if (scheduleMode == ScheduleMode.ParallelUnsafe) { - var entity = layer.bodies[i].entity; - throw new InvalidOperationException( - $"A parallel FindPairs job was scheduled using a layer containing more than one instance of Entity {entity}"); + if (index < layer.bucketCount) + { + Physics.kCellMarker.Begin(); + var bucket = layer.GetBucketSlices(index); + var context = new FindPairsBucketContext(in layer, + in layer, + in bucket, + in bucket, + index, + false, + false); + if (processor.BeginBucket(in context)) + { + if (index != layer.bucketCount - 1) + FindPairsSweepMethods.SelfSweepCell(in layer, in bucket, index, ref processor, false, false); + else + FindPairsSweepMethods.SelfSweepCross(in layer, in bucket, index, ref processor, false, false); + processor.EndBucket(in context); + } + Physics.kCellMarker.End(); + } + else + { + Physics.kCrossMarker.Begin(); + var i = index - layer.bucketCount; + var bucket = layer.GetBucketSlices(i); + var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); + var context = new FindPairsBucketContext(in layer, + in layer, + in bucket, + in crossBucket, + layer.bucketCount + i, + false, + false); + if (processor.BeginBucket(in context)) + { + FindPairsSweepMethods.BipartiteSweepCellCross(layer, layer, bucket, crossBucket, i + layer.bucketCount, ref processor, false, false); + processor.EndBucket(in context); + } + Physics.kCrossMarker.End(); + } } } - } -#endif - - #endregion - } - } - - public partial struct FindPairsLayerSelfWithCrossCacheConfig where T : struct, IFindPairsProcessor - { - internal static class FindPairsInternal - { - #region Jobs - - // Schedule for (2 * layer.bucketCount - 1) iterations - [BurstCompile] - public struct LayerSelfPart1 : IJobParallelFor, IFindPairsProcessor - { - [ReadOnly] public CollisionLayer layer; - public T processor; - [NativeDisableParallelForRestriction] public NativeStream.Writer cache; - public void Execute(int index) + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + private static void EntityAliasCheck(in CollisionLayer layer) { - if (index < layer.bucketCount) - { - var bucket = layer.GetBucketSlices(index); - var context = new FindPairsBucketContext(in layer, in layer, bucket.bucketGlobalStart, bucket.count, bucket.bucketGlobalStart, bucket.count, index, true); - processor.BeginBucket(in context); - FindPairsSweepMethods.SelfSweep(layer, bucket, index, ref processor); - processor.EndBucket(in context); - } - else + var hashSet = new NativeParallelHashSet(layer.count, Allocator.Temp); + for (int i = 0; i < layer.count; i++) { - var bucket = layer.GetBucketSlices(index - layer.bucketCount); - var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); - - cache.BeginForEachIndex(index - layer.bucketCount); - FindPairsSweepMethods.BipartiteSweep(layer, layer, bucket, crossBucket, index, ref this); - cache.EndForEachIndex(); + if (!hashSet.Add(layer.bodies[i].entity)) + { + var entity = layer.bodies[i].entity; + throw new InvalidOperationException( + $"A parallel FindPairs job was scheduled using a layer containing more than one instance of Entity {entity}"); + } } } + #endregion - public void Execute(in FindPairsResult result) + [Preserve] + void RequireEarlyJobInit() { - int2 pair = new int2(result.bodyIndexA, result.bodyIndexB); - cache.Write(pair); + new InitJobsForProcessors.FindPairsIniter().Init(); } } - [BurstCompile] - public struct LayerSelfPart2 : IJob + public static void RunImmediate(in CollisionLayer layer, ref T processor, bool isThreadSafe) { - [ReadOnly] public CollisionLayer layer; - public T processor; - [ReadOnly] public NativeStream.Reader cache; - - public void Execute() + int jobIndex = 0; + for (int i = 0; i < layer.bucketCount; i++) { - for (int i = 0; i < layer.bucketCount - 1; i++) - { - var result = FindPairsResult.CreateGlobalResult(layer, layer, layer.bucketCount + i, true); - var bucket = layer.bucketStartsAndCounts[i]; - var crossBucket = layer.bucketStartsAndCounts[layer.bucketCount - 1]; - var context = new FindPairsBucketContext(in layer, - in layer, - bucket.x, - bucket.y, - crossBucket.x, - crossBucket.y, - layer.bucketCount + i, - true); - processor.BeginBucket(in context); - - var count = cache.BeginForEachIndex(i); - for (; count > 0; count--) - { - var pair = cache.Read(); - result.SetBucketRelativePairIndices(pair.x, pair.y); - processor.Execute(in result); - } - cache.EndForEachIndex(); - + var bucket = layer.GetBucketSlices(i); + var context = new FindPairsBucketContext(in layer, + in layer, + in bucket, + in bucket, + jobIndex, + isThreadSafe, + isThreadSafe, + !isThreadSafe); + if (processor.BeginBucket(in context)) + { + if (i != layer.bucketCount - 1) + FindPairsSweepMethods.SelfSweepCell(in layer, in bucket, jobIndex, ref processor, isThreadSafe, isThreadSafe, !isThreadSafe); + else + FindPairsSweepMethods.SelfSweepCross(in layer, in bucket, jobIndex, ref processor, isThreadSafe, isThreadSafe, !isThreadSafe); processor.EndBucket(in context); } + jobIndex++; } - } - - #endregion - - #region SafeChecks - -#if ENABLE_UNITY_COLLECTIONS_CHECKS - - // Schedule for 2 iterations - [BurstCompile] - public struct LayerSelfPart2_WithSafety : IJobFor - { - [ReadOnly] public CollisionLayer layer; - public T processor; - [ReadOnly] public NativeStream.Reader cache; - - public void Execute(int index) - { - if (index == 0) - { - for (int i = 0; i < layer.bucketCount - 1; i++) - { - var result = FindPairsResult.CreateGlobalResult(layer, layer, layer.bucketCount + i, true); - var bucket = layer.bucketStartsAndCounts[i]; - var crossBucket = layer.bucketStartsAndCounts[layer.bucketCount - 1]; - var context = new FindPairsBucketContext(in layer, - in layer, - bucket.x, - bucket.y, - crossBucket.x, - crossBucket.y, - layer.bucketCount + i, - true); - processor.BeginBucket(in context); - - var count = cache.BeginForEachIndex(i); - for (; count > 0; count--) - { - var pair = cache.Read(); - result.SetBucketRelativePairIndices(pair.x, pair.y); - processor.Execute(in result); - } - cache.EndForEachIndex(); - - processor.EndBucket(in context); - } - } - else - { - EntityAliasCheck(layer); - } - } - } - [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] - private static void EntityAliasCheck(CollisionLayer layer) - { - var hashSet = new NativeParallelHashSet(layer.count, Allocator.Temp); - for (int i = 0; i < layer.count; i++) + var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); + for (int i = 0; i < layer.bucketCount - 1; i++) { - if (!hashSet.Add(layer.bodies[i].entity)) + var bucket = layer.GetBucketSlices(i); + var context = new FindPairsBucketContext(in layer, + in layer, + in bucket, + in crossBucket, + jobIndex, + isThreadSafe, + isThreadSafe, + !isThreadSafe); + if (processor.BeginBucket(in context)) { - var entity = layer.bodies[i].entity; - throw new InvalidOperationException( - $"A parallel FindPairs job was scheduled using a layer containing more than one instance of Entity {entity}"); + FindPairsSweepMethods.BipartiteSweepCellCross(in layer, + in layer, + in bucket, + in crossBucket, + jobIndex, + ref processor, + isThreadSafe, + isThreadSafe, + !isThreadSafe); + processor.EndBucket(in context); } + jobIndex++; } } -#endif - - #endregion } } public partial struct FindPairsLayerLayerConfig where T : struct, IFindPairsProcessor { + internal enum ScheduleMode + { + Single = 0x0, + ParallelPart1 = 0x1, + ParallelPart2 = 0x2, + ParallelByA = 0x3, + ParallelUnsafe = 0x4, + UseCrossCache = 0x40, + AllowEntityAliasing = 0x80, + } + internal static class FindPairsInternal { - #region Jobs [BurstCompile] - public struct LayerLayerSingle : IJob + public struct LayerLayerJob : IJobFor { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; + [ReadOnly] CollisionLayer layerA; + [ReadOnly] CollisionLayer layerB; + T processor; + UnsafeIndexedBlockList blockList; + ScheduleMode m_scheduleMode; + Unity.Profiling.ProfilerMarker modeAndTMarker; - public void Execute() + #region Construction and Scheduling + public LayerLayerJob(in CollisionLayer layerA, in CollisionLayer layerB, in T processor) { - RunImmediate(in layerA, in layerB, ref processor, true); + this.layerA = layerA; + this.layerB = layerB; + this.processor = processor; + blockList = default; + m_scheduleMode = default; + modeAndTMarker = default; } - } - - [BurstCompile] - public struct LayerLayerPart1 : IJobParallelFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; - public void Execute(int index) + public void Run() { - var bucketA = layerA.GetBucketSlices(index); - var bucketB = layerB.GetBucketSlices(index); - var context = new FindPairsBucketContext(in layerA, in layerB, bucketA.bucketGlobalStart, bucketA.count, bucketB.bucketGlobalStart, bucketB.count, index, true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, bucketA, bucketB, index, ref processor); - processor.EndBucket(in context); + SetScheduleMode(ScheduleMode.Single); + this.Run(1); } - } - // Schedule for 2 iterations - [BurstCompile] - public struct LayerLayerPart2 : IJobParallelFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; + public JobHandle ScheduleSingle(JobHandle inputDeps) + { + SetScheduleMode(ScheduleMode.Single); + return this.Schedule(1, inputDeps); + } - public void Execute(int index) + public JobHandle ScheduleParallel(JobHandle inputDeps, ScheduleMode scheduleMode) { - if (index == 0) + SetScheduleMode(scheduleMode); + scheduleMode = ExtractEnum(scheduleMode, out var useCrossCache, out var allowEntityAliasing); + + if (scheduleMode == ScheduleMode.ParallelPart1) { - var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); - for (int i = 0; i < layerB.bucketCount - 1; i++) - { - var bucket = layerB.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layerA, - in layerB, - crossBucket.bucketGlobalStart, - crossBucket.count, - bucket.bucketGlobalStart, - bucket.count, - layerA.bucketCount + i, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, crossBucket, bucket, layerA.bucketCount + i, ref processor); - processor.EndBucket(in context); - } + return this.ScheduleParallel(layerA.bucketCount, 1, inputDeps); } - else if (index == 1) + if (scheduleMode == ScheduleMode.ParallelPart2 && allowEntityAliasing) + return this.ScheduleParallel(2, 1, inputDeps); + if (scheduleMode == ScheduleMode.ParallelPart2) +#if ENABLE_UNITY_COLLECTIONS_CHECKS + return this.ScheduleParallel(3, 1, inputDeps); +#else + return this.ScheduleParallel(2, 1, inputDeps); +#endif + if (scheduleMode == ScheduleMode.ParallelByA) { - var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); - for (int i = 0; i < layerA.bucketCount - 1; i++) + if (useCrossCache) { - var bucket = layerA.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layerA, - in layerB, - bucket.bucketGlobalStart, - bucket.count, - crossBucket.bucketGlobalStart, - crossBucket.count, - layerA.bucketCount + layerB.bucketCount - 1 + i, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, bucket, crossBucket, layerA.bucketCount + layerB.bucketCount - 1 + i, ref processor); - processor.EndBucket(in context); + blockList = new UnsafeIndexedBlockList(8, 1024, layerA.bucketCount - 1, Allocator.TempJob); + inputDeps = new FindPairsParallelByACrossCacheJob { layerA = layerA, layerB = layerB, cache = blockList }.ScheduleParallel(layerA.bucketCount - 1, + 1, + inputDeps); + scheduleMode |= ScheduleMode.UseCrossCache; } + + if (allowEntityAliasing) + inputDeps = this.ScheduleParallel(layerA.bucketCount, 1, inputDeps); + else +#if ENABLE_UNITY_COLLECTIONS_CHECKS + inputDeps = this.ScheduleParallel(layerA.bucketCount + 1, 1, inputDeps); +#else + inputDeps = this.ScheduleParallel(layerA.bucketCount, 1, inputDeps); +#endif + if (useCrossCache) + return blockList.Dispose(inputDeps); + return inputDeps; } + if (scheduleMode == ScheduleMode.ParallelUnsafe) + return this.ScheduleParallel(layerA.bucketCount * 3 - 2, 1, inputDeps); + return inputDeps; } - } - - // Schedule for (3 * layer.bucketCount - 2) iterations - [BurstCompile] - public struct LayerLayerParallelUnsafe : IJobFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; - public void Execute(int i) + void SetScheduleMode(ScheduleMode scheduleMode) { - if (i < layerA.bucketCount) + m_scheduleMode = scheduleMode; + scheduleMode = ExtractEnum(scheduleMode, out var useCrossCache, out var allowEntityAliasing); + FixedString64Bytes modeString = default; + if (scheduleMode == ScheduleMode.Single) + modeString = "Single"; + else if (scheduleMode == ScheduleMode.ParallelPart1) + modeString = "ParallelPart1"; + else if (scheduleMode == ScheduleMode.ParallelPart2) + modeString = "ParallelPart2"; + else if (scheduleMode == ScheduleMode.ParallelByA) + modeString = "ParallelByA"; + else if (scheduleMode == ScheduleMode.ParallelUnsafe) + modeString = "ParallelUnsafe"; + + FixedString32Bytes appendString; + if (useCrossCache) { - var bucketA = layerA.GetBucketSlices(i); - var bucketB = layerB.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layerA, in layerB, bucketA.bucketGlobalStart, bucketA.count, bucketB.bucketGlobalStart, bucketB.count, i, true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, bucketA, bucketB, i, ref processor); - processor.EndBucket(in context); + appendString = "_CrossCache"; + modeString.Append(appendString); } - else if (i < 2 * layerB.bucketCount - 1) + if (allowEntityAliasing) { - i -= layerB.bucketCount; - var bucket = layerB.GetBucketSlices(i); - var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); - var context = new FindPairsBucketContext(in layerA, - in layerB, - crossBucket.bucketGlobalStart, - crossBucket.count, - bucket.bucketGlobalStart, - bucket.count, - layerA.bucketCount + i, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, crossBucket, bucket, i + layerB.bucketCount, ref processor); - processor.EndBucket(in context); + appendString = "_EntityAliasing"; + modeString.Append(appendString); + } + + bool isBurst = true; + IsBurst(ref isBurst); + if (isBurst) + { + modeAndTMarker = new Unity.Profiling.ProfilerMarker($"{modeString}_{processor}"); } else { - var jobIndex = i; - i -= (2 * layerB.bucketCount - 1); - var bucket = layerA.GetBucketSlices(i); - var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); - var context = new FindPairsBucketContext(in layerA, - in layerB, - bucket.bucketGlobalStart, - bucket.count, - crossBucket.bucketGlobalStart, - crossBucket.count, - jobIndex, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, bucket, crossBucket, jobIndex, ref processor); - processor.EndBucket(in context); + FixedString128Bytes processorName = default; + GetProcessorNameNoBurst(ref processorName); + modeAndTMarker = new Unity.Profiling.ProfilerMarker($"{modeString}_{processorName}"); } } - } - #endregion - - #region ImmediateMethods - public static void RunImmediate(in CollisionLayer layerA, in CollisionLayer layerB, ref T processor, bool isThreadSafe) - { - int jobIndex = 0; - for (int i = 0; i < layerA.bucketCount; i++) + ScheduleMode ExtractEnum(ScheduleMode mode, out bool useCrossCache, out bool allowEntityAliasing) { - var bucketA = layerA.GetBucketSlices(i); - var bucketB = layerB.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layerA, - in layerB, - bucketA.bucketGlobalStart, - bucketA.count, - bucketB.bucketGlobalStart, - bucketB.count, - jobIndex, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, bucketA, bucketB, jobIndex, ref processor, isThreadSafe); - processor.EndBucket(in context); - jobIndex++; + allowEntityAliasing = (mode & ScheduleMode.AllowEntityAliasing) == ScheduleMode.AllowEntityAliasing; + useCrossCache = (mode & ScheduleMode.UseCrossCache) == ScheduleMode.UseCrossCache; + return mode & ~(ScheduleMode.AllowEntityAliasing | ScheduleMode.UseCrossCache); } - var crossBucketA = layerA.GetBucketSlices(layerA.bucketCount - 1); - for (int i = 0; i < layerA.bucketCount - 1; i++) - { - var bucket = layerB.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layerA, - in layerB, - crossBucketA.bucketGlobalStart, - crossBucketA.count, - bucket.bucketGlobalStart, - bucket.count, - jobIndex, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, crossBucketA, bucket, jobIndex, ref processor, isThreadSafe); - processor.EndBucket(in context); - jobIndex++; - } + [BurstDiscard] + static void IsBurst(ref bool isBurst) => isBurst = false; - var crossBucketB = layerB.GetBucketSlices(layerB.bucketCount - 1); - for (int i = 0; i < layerA.bucketCount - 1; i++) + [BurstDiscard] + static void GetProcessorNameNoBurst(ref FixedString128Bytes name) { - var bucket = layerA.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layerA, - in layerB, - bucket.bucketGlobalStart, - bucket.count, - crossBucketB.bucketGlobalStart, - crossBucketB.count, - jobIndex, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, bucket, crossBucketB, jobIndex, ref processor, isThreadSafe); - processor.EndBucket(in context); - jobIndex++; + name = nameof(T); } - } - #endregion - - #region SafeChecks - -#if ENABLE_UNITY_COLLECTIONS_CHECKS - - // Schedule for 3 iterations - [BurstCompile] - public struct LayerLayerPart2_WithSafety : IJobParallelFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; + #endregion + #region Job Processing public void Execute(int index) { - if (index == 0) - { - var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); - for (int i = 0; i < layerB.bucketCount - 1; i++) + using var jobName = modeAndTMarker.Auto(); + var scheduleMode = ExtractEnum(m_scheduleMode, out var useCrossCache, out var allowEntityAliasing); + if (scheduleMode == ScheduleMode.Single) + { + RunImmediate(in layerA, layerB, ref processor, true); + return; + } + if (scheduleMode == ScheduleMode.ParallelPart1) + { + bool isThreadSafe = !allowEntityAliasing; + Physics.kCellMarker.Begin(); + var bucketA = layerA.GetBucketSlices(index); + var bucketB = layerB.GetBucketSlices(index); + var context = new FindPairsBucketContext(in layerA, + in layerB, + in bucketA, + in bucketB, + index, + isThreadSafe, + isThreadSafe); + if (processor.BeginBucket(in context)) { - var bucket = layerB.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layerA, - in layerB, - crossBucket.bucketGlobalStart, - crossBucket.count, - bucket.bucketGlobalStart, - bucket.count, - layerA.bucketCount + i, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, crossBucket, bucket, layerA.bucketCount + i, ref processor); + if (index != layerA.bucketCount - 1) + FindPairsSweepMethods.BipartiteSweepCellCell(in layerA, in layerB, in bucketA, in bucketB, index, ref processor, isThreadSafe, isThreadSafe); + else + FindPairsSweepMethods.BipartiteSweepCrossCross(in layerA, in layerB, in bucketA, in bucketB, index, ref processor, isThreadSafe, isThreadSafe); processor.EndBucket(in context); } + Physics.kCellMarker.End(); + return; } - else if (index == 1) + if (scheduleMode == ScheduleMode.ParallelPart2) { - var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); - for (int i = 0; i < layerA.bucketCount - 1; i++) + if (index == 2) { - var bucket = layerA.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layerA, - in layerB, - bucket.bucketGlobalStart, - bucket.count, - crossBucket.bucketGlobalStart, - crossBucket.count, - layerA.bucketCount + layerB.bucketCount - 1 + i, - true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, bucket, crossBucket, layerA.bucketCount + layerB.bucketCount - 1 + i, ref processor); - processor.EndBucket(in context); + EntityAliasCheck(in layerA, in layerB); + return; } - } - else - { - EntityAliasCheck(layerA, layerB); - } - } - } - - [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] - private static void EntityAliasCheck(CollisionLayer layerA, CollisionLayer layerB) - { - var hashSet = new NativeParallelHashSet(layerA.count + layerB.count, Allocator.Temp); - for (int i = 0; i < layerA.count; i++) - { - if (!hashSet.Add(layerA.bodies[i].entity)) - { - //Note: At this point, we know the issue lies exclusively in layerA. - var entity = layerA.bodies[i].entity; - throw new InvalidOperationException( - $"A parallel FindPairs job was scheduled using a layer containing more than one instance of Entity {entity}"); - } - } - for (int i = 0; i < layerB.count; i++) - { - if (!hashSet.Add(layerB.bodies[i].entity)) - { - //Note: At this point, it is unknown whether the repeating entity first showed up in layerA or layerB. - var entity = layerB.bodies[i].entity; - throw new InvalidOperationException( - $"A parallel FindPairs job was scheduled using two layers combined containing more than one instance of Entity {entity}"); - } - } - } -#endif - - #endregion - } - } - - public partial struct FindPairsLayerLayerWithCrossCacheConfig where T : struct, IFindPairsProcessor - { - internal static class FindPairsInternal - { - #region Jobs - - // Schedule for (3 * layer.bucketCount - 2) iterations - [BurstCompile] - public struct LayerLayerPart1 : IJobParallelFor, IFindPairsProcessor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; - [NativeDisableParallelForRestriction] public NativeStream.Writer cache; - - public void Execute(int i) - { - if (i < layerA.bucketCount) - { - var bucketA = layerA.GetBucketSlices(i); - var bucketB = layerB.GetBucketSlices(i); - var context = new FindPairsBucketContext(in layerA, in layerB, bucketA.bucketGlobalStart, bucketA.count, bucketB.bucketGlobalStart, bucketB.count, i, true); - processor.BeginBucket(in context); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, bucketA, bucketB, i, ref processor); - processor.EndBucket(in context); - } - else if (i < 2 * layerB.bucketCount - 1) - { - i -= layerB.bucketCount; - var bucket = layerB.GetBucketSlices(i); - var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); - cache.BeginForEachIndex(i); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, crossBucket, bucket, i + layerB.bucketCount, ref this); - cache.EndForEachIndex(); - } - else - { - var jobIndex = i; - i -= (2 * layerB.bucketCount - 1); - var bucket = layerA.GetBucketSlices(i); - var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); - cache.BeginForEachIndex(i + layerB.bucketCount - 1); - FindPairsSweepMethods.BipartiteSweep(layerA, layerB, bucket, crossBucket, jobIndex, ref this); - cache.EndForEachIndex(); - } - } - - public void Execute(in FindPairsResult result) - { - int2 pair = new int2(result.bodyIndexA, result.bodyIndexB); - cache.Write(pair); - } - } - - // Schedule for 2 iterations - [BurstCompile] - public struct LayerLayerPart2 : IJobParallelFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; - public NativeStream.Reader cache; - public void Execute(int index) - { - if (index == 0) - { - for (int i = 0; i < layerB.bucketCount - 1; i++) + bool isThreadSafe = !allowEntityAliasing; + Physics.kCrossMarker.Begin(); + if (index == 0) { - var result = FindPairsResult.CreateGlobalResult(layerA, layerB, layerA.bucketCount + i, true); - var bucket = layerB.bucketStartsAndCounts[i]; - var crossBucket = layerA.bucketStartsAndCounts[layerA.bucketCount - 1]; - var context = new FindPairsBucketContext(in layerA, + var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); + for (int i = 0; i < layerB.bucketCount - 1; i++) + { + var bucket = layerB.GetBucketSlices(i); + var context = new FindPairsBucketContext(in layerA, in layerB, - crossBucket.x, - crossBucket.y, - bucket.x, - bucket.y, + in crossBucket, + in bucket, layerA.bucketCount + i, - true); - processor.BeginBucket(in context); - - var count = cache.BeginForEachIndex(i); - for (; count > 0; count--) + isThreadSafe, + isThreadSafe); + if (processor.BeginBucket(in context)) + { + FindPairsSweepMethods.BipartiteSweepCrossCell(layerA, layerB, crossBucket, bucket, layerA.bucketCount + i, ref processor, isThreadSafe, + isThreadSafe); + processor.EndBucket(in context); + } + } + } + else if (index == 1) + { + var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); + for (int i = 0; i < layerA.bucketCount - 1; i++) { - var pair = cache.Read(); - result.SetBucketRelativePairIndices(pair.x, pair.y); - processor.Execute(in result); + var bucket = layerA.GetBucketSlices(i); + var context = new FindPairsBucketContext(in layerA, + in layerB, + in bucket, + in crossBucket, + layerA.bucketCount * 2 - 1 + i, + isThreadSafe, + isThreadSafe); + if (processor.BeginBucket(in context)) + { + FindPairsSweepMethods.BipartiteSweepCellCross(layerA, + layerB, + bucket, + crossBucket, + layerA.bucketCount * 2 - 1 + i, + ref processor, + isThreadSafe, + isThreadSafe); + processor.EndBucket(in context); + } } - cache.EndForEachIndex(); - - processor.EndBucket(in context); } + Physics.kCrossMarker.End(); + return; } - else if (index == 1) + if (scheduleMode == ScheduleMode.ParallelByA) { - for (int i = 0; i < layerA.bucketCount - 1; i++) + if (index == layerA.bucketCount) { - var result = FindPairsResult.CreateGlobalResult(layerA, layerB, layerA.bucketCount + layerB.bucketCount - 1 + i, true); - - var bucket = layerA.bucketStartsAndCounts[i]; - var crossBucket = layerB.bucketStartsAndCounts[layerB.bucketCount - 1]; - var context = new FindPairsBucketContext(in layerA, - in layerB, - bucket.x, - bucket.y, - crossBucket.x, - crossBucket.y, - layerA.bucketCount + layerB.bucketCount - 1 + i, - true); - processor.BeginBucket(in context); - - var count = cache.BeginForEachIndex(i + layerA.bucketCount - 1); - for (; count > 0; count--) - { - var pair = cache.Read(); - result.SetBucketRelativePairIndices(pair.x, pair.y); - processor.Execute(in result); - } - cache.EndForEachIndex(); + EntityAliasCheck(in layerA, in layerB); + return; + } + bool isThreadSafe = !allowEntityAliasing; + Physics.kCellMarker.Begin(); + var bucketA = layerA.GetBucketSlices(index); + var bucketB = layerB.GetBucketSlices(index); + var context = new FindPairsBucketContext(in layerA, + in layerB, + in bucketA, + in bucketB, + index, + isThreadSafe, + false); + if (processor.BeginBucket(in context)) + { + if (index != layerA.bucketCount - 1) + FindPairsSweepMethods.BipartiteSweepCellCell(in layerA, in layerB, in bucketA, in bucketB, index, ref processor, isThreadSafe, false); + else + FindPairsSweepMethods.BipartiteSweepCrossCross(in layerA, in layerB, in bucketA, in bucketB, index, ref processor, isThreadSafe, false); processor.EndBucket(in context); } - } - } - } - - #endregion + Physics.kCellMarker.End(); - #region SafeChecks - -#if ENABLE_UNITY_COLLECTIONS_CHECKS - - // Schedule for 3 iterations - [BurstCompile] - public struct LayerLayerPart2_WithSafety : IJobParallelFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; - public NativeStream.Reader cache; - - public void Execute(int index) - { - if (index == 0) - { - for (int i = 0; i < layerB.bucketCount - 1; i++) + Physics.kCrossMarker.Begin(); + if (index < layerA.bucketCount - 1) { - var result = FindPairsResult.CreateGlobalResult(layerA, layerB, layerA.bucketCount + i, true); - var bucket = layerB.bucketStartsAndCounts[i]; - var crossBucket = layerA.bucketStartsAndCounts[layerA.bucketCount - 1]; - var context = new FindPairsBucketContext(in layerA, + var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); + context = new FindPairsBucketContext(in layerA, in layerB, - crossBucket.x, - crossBucket.y, - bucket.x, - bucket.y, - layerA.bucketCount + i, - true); - processor.BeginBucket(in context); - - var count = cache.BeginForEachIndex(i); - for (; count > 0; count--) + in bucketA, + in crossBucket, + layerA.bucketCount * 2 - 1 + index, + isThreadSafe, + false); + if (processor.BeginBucket(in context)) { - var pair = cache.Read(); - result.SetBucketRelativePairIndices(pair.x, pair.y); - processor.Execute(in result); + FindPairsSweepMethods.BipartiteSweepCellCross(in layerA, + in layerB, + in bucketA, + in crossBucket, + layerA.bucketCount * 2 - 1 + index, + ref processor, + isThreadSafe, + false); + processor.EndBucket(in context); } - cache.EndForEachIndex(); } - } - else if (index == 1) - { - for (int i = 0; i < layerA.bucketCount - 1; i++) + else if (useCrossCache) { - var result = FindPairsResult.CreateGlobalResult(layerA, layerB, layerA.bucketCount + layerB.bucketCount - 1 + i, true); - var bucket = layerA.bucketStartsAndCounts[i]; - var crossBucket = layerB.bucketStartsAndCounts[layerB.bucketCount - 1]; - var context = new FindPairsBucketContext(in layerA, - in layerB, - bucket.x, - bucket.y, - crossBucket.x, - crossBucket.y, - layerA.bucketCount + layerB.bucketCount - 1 + i, - true); - processor.BeginBucket(in context); - - var count = cache.BeginForEachIndex(i + layerA.bucketCount - 1); - for (; count > 0; count--) + var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); + for (int i = 0; i < layerB.bucketCount - 1; i++) { - var pair = cache.Read(); - result.SetBucketRelativePairIndices(pair.x, pair.y); - processor.Execute(in result); + var bucket = layerB.GetBucketSlices(i); + context = new FindPairsBucketContext(in layerA, + in layerB, + in crossBucket, + in bucket, + layerA.bucketCount + i, + isThreadSafe, + isThreadSafe); + if (processor.BeginBucket(in context)) + { + var enumerator = blockList.GetEnumerator(i); + //var tempMarker = new ProfilerMarker($"Cache_{i}"); + //tempMarker.Begin(); + var count = FindPairsSweepMethods.BipartiteSweepPlayCache(enumerator, + in layerA, + in layerB, + crossBucket.bucketIndex, + bucket.bucketIndex, + layerA.bucketCount + i, + ref processor, + isThreadSafe, + false); + //tempMarker.End(); + processor.EndBucket(in context); + } + + //if (count > 0) + // UnityEngine.Debug.Log($"Bucket cache had count {count}"); } - cache.EndForEachIndex(); - - processor.EndBucket(in context); } - } - else - { - EntityAliasCheck(layerA, layerB); - } - } - } - - [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] - private static void EntityAliasCheck(CollisionLayer layerA, CollisionLayer layerB) - { - var hashSet = new NativeParallelHashSet(layerA.count + layerB.count, Allocator.Temp); - for (int i = 0; i < layerA.count; i++) - { - if (!hashSet.Add(layerA.bodies[i].entity)) - { - //Note: At this point, we know the issue lies exclusively in layerA. - var entity = layerA.bodies[i].entity; - throw new InvalidOperationException( - $"A parallel FindPairs job was scheduled using a layer containing more than one instance of Entity {entity}"); - } - } - for (int i = 0; i < layerB.count; i++) - { - if (!hashSet.Add(layerB.bodies[i].entity)) - { - //Note: At this point, it is unknown whether the repeating entity first showed up in layerA or layerB. - var entity = layerB.bodies[i].entity; - throw new InvalidOperationException( - $"A parallel FindPairs job was scheduled using two layers combined containing more than one instance of Entity {entity}"); - } - } - } -#endif - - #endregion - } - } - - internal partial struct FindPairsLayerSelfConfigUnrolled where T : struct, IFindPairsProcessor - { - internal static class FindPairsInternalUnrolled - { - #region Jobs - [BurstCompile] - public struct LayerSelfSingle : IJob - { - [ReadOnly] public CollisionLayer layer; - public T processor; - - public void Execute() - { - RunImmediate(layer, processor); - } - } - - [BurstCompile] - public struct LayerSelfPart1 : IJobParallelFor - { - [ReadOnly] public CollisionLayer layer; - public T processor; - - public void Execute(int index) - { - var bucket = layer.GetBucketSlices(index); - var drain = new FindPairsSweepMethods.FindPairsProcessorDrain(); - //drain.drainBuffer1024 = new NativeArray(1024, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - drain.processor = processor; - FindPairsSweepMethods.SelfSweepUnrolled(layer, bucket, index, ref drain); - } - } - - [BurstCompile] - public struct LayerSelfPart2 : IJob - { - [ReadOnly] public CollisionLayer layer; - public T processor; - - public void Execute() - { - var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); - var drain = new FindPairsSweepMethods.FindPairsProcessorDrain(); - //drain.drainBuffer1024 = new NativeArray(1024, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - drain.processor = processor; - for (int i = 0; i < layer.bucketCount - 1; i++) - { - var bucket = layer.GetBucketSlices(i); - FindPairsSweepMethods.BipartiteSweepUnrolled(layer, layer, bucket, crossBucket, layer.bucketCount + i, ref drain); - } - } - } - - [BurstCompile] - public struct LayerSelfParallelUnsafe : IJobFor - { - [ReadOnly] public CollisionLayer layer; - public T processor; - - public void Execute(int i) - { - var drain = new FindPairsSweepMethods.FindPairsProcessorDrain(); - //drain.drainBuffer1024 = new NativeArray(1024, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - drain.processor = processor; - - if (i < layer.bucketCount) - { - var bucket = layer.GetBucketSlices(i); - FindPairsSweepMethods.SelfSweepUnrolled(layer, bucket, i, ref drain); - } - else - { - i -= layer.bucketCount; - var bucket = layer.GetBucketSlices(i); - var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); - FindPairsSweepMethods.BipartiteSweepUnrolled(layer, layer, bucket, crossBucket, i + layer.bucketCount, ref drain); - } - } - } - #endregion - - #region ImmediateMethods - public static void RunImmediate(CollisionLayer layer, T processor) - { - var drain = new FindPairsSweepMethods.FindPairsProcessorDrain(); - //drain.drainBuffer1024 = new NativeArray(1024, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - drain.processor = processor; - - int jobIndex = 0; - for (int i = 0; i < layer.bucketCount; i++) - { - var bucket = layer.GetBucketSlices(i); - FindPairsSweepMethods.SelfSweepUnrolled(layer, bucket, jobIndex++, ref drain, false); - } - - var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); - for (int i = 0; i < layer.bucketCount - 1; i++) - { - var bucket = layer.GetBucketSlices(i); - FindPairsSweepMethods.BipartiteSweepUnrolled(layer, layer, bucket, crossBucket, jobIndex++, ref drain, false); - } - } - #endregion - - #region SafeChecks - -#if ENABLE_UNITY_COLLECTIONS_CHECKS - [BurstCompile] - public struct LayerSelfPart2_WithSafety : IJobFor - { - [ReadOnly] public CollisionLayer layer; - public T processor; - - public void Execute(int index) - { - if (index == 0) - { - var drain = new FindPairsSweepMethods.FindPairsProcessorDrain(); - //drain.drainBuffer1024 = new NativeArray(1024, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - drain.processor = processor; - - var crossBucket = layer.GetBucketSlices(layer.bucketCount - 1); - for (int i = 0; i < layer.bucketCount - 1; i++) + else { - var bucket = layer.GetBucketSlices(i); - FindPairsSweepMethods.BipartiteSweepUnrolled(layer, layer, bucket, crossBucket, layer.bucketCount + i, ref drain); + var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); + for (int i = 0; i < layerB.bucketCount - 1; i++) + { + var bucket = layerB.GetBucketSlices(i); + context = new FindPairsBucketContext(in layerA, + in layerB, + in crossBucket, + in bucket, + layerA.bucketCount + i, + isThreadSafe, + isThreadSafe); + if (processor.BeginBucket(in context)) + { + FindPairsSweepMethods.BipartiteSweepCrossCell(layerA, layerB, crossBucket, bucket, layerA.bucketCount + i, ref processor, isThreadSafe, false); + processor.EndBucket(in context); + } + } } + Physics.kCrossMarker.End(); + return; } - else - { - EntityAliasCheck(layer); - } - } - } - - [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] - private static void EntityAliasCheck(CollisionLayer layer) - { - var hashSet = new NativeParallelHashSet(layer.count, Allocator.Temp); - for (int i = 0; i < layer.count; i++) - { - if (!hashSet.Add(layer.bodies[i].entity)) + if (scheduleMode == ScheduleMode.ParallelUnsafe) { - var entity = layer.bodies[i].entity; - throw new InvalidOperationException( - $"A parallel FindPairs job was scheduled using a layer containing more than one instance of Entity {entity}"); + if (index < layerA.bucketCount) + { + Physics.kCellMarker.Begin(); + var bucketA = layerA.GetBucketSlices(index); + var bucketB = layerB.GetBucketSlices(index); + var context = new FindPairsBucketContext(in layerA, + in layerB, + in bucketA, + in bucketB, + index, + false, + false); + if (processor.BeginBucket(in context)) + { + if (index != layerA.bucketCount - 1) + FindPairsSweepMethods.BipartiteSweepCellCell(in layerA, in layerB, in bucketA, in bucketB, index, ref processor, false, false); + else + FindPairsSweepMethods.BipartiteSweepCrossCross(in layerA, in layerB, in bucketA, in bucketB, index, ref processor, false, false); + processor.EndBucket(in context); + } + Physics.kCellMarker.End(); + } + else if (index < 2 * layerB.bucketCount - 1) + { + Physics.kCrossMarker.Begin(); + index -= layerB.bucketCount; + var bucket = layerB.GetBucketSlices(index); + var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); + var context = new FindPairsBucketContext(in layerA, + in layerB, + in crossBucket, + in bucket, + layerA.bucketCount + index, + false, + false); + if (processor.BeginBucket(in context)) + { + FindPairsSweepMethods.BipartiteSweepCrossCell(in layerA, + in layerB, + in crossBucket, + in bucket, + index + layerB.bucketCount, + ref processor, + false, + false); + processor.EndBucket(in context); + } + Physics.kCrossMarker.End(); + } + else + { + Physics.kCrossMarker.Begin(); + var jobIndex = index; + index -= (2 * layerB.bucketCount - 1); + var bucket = layerA.GetBucketSlices(index); + var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); + var context = new FindPairsBucketContext(in layerA, + in layerB, + in bucket, + in crossBucket, + jobIndex, + false, + false); + if (processor.BeginBucket(in context)) + { + FindPairsSweepMethods.BipartiteSweepCellCross(in layerA, in layerB, in bucket, in crossBucket, jobIndex, ref processor, false, false); + processor.EndBucket(in context); + } + Physics.kCrossMarker.End(); + } + return; } } - } -#endif - - #endregion - - #region SweepUnrolledMethods - - #endregion SweepUnrolledMethods - } - } - - internal partial struct FindPairsLayerLayerConfigUnrolled where T : struct, IFindPairsProcessor - { - internal static class FindPairsInternalUnrolled - { - #region Jobs - [BurstCompile] - public struct LayerLayerSingle : IJob - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; - - public void Execute() - { - RunImmediate(layerA, layerB, processor); - } - } - - [BurstCompile] - public struct LayerLayerPart1 : IJobParallelFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; - - public void Execute(int index) - { - var bucketA = layerA.GetBucketSlices(index); - var bucketB = layerB.GetBucketSlices(index); - - var drain = new FindPairsSweepMethods.FindPairsProcessorDrain(); - //drain.drainBuffer1024 = new NativeArray(1024, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - drain.processor = processor; - - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, bucketA, bucketB, index, ref drain); - } - } - [BurstCompile] - public struct LayerLayerPart2 : IJobParallelFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; - - public void Execute(int index) + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + private static void EntityAliasCheck(in CollisionLayer layerA, in CollisionLayer layerB) { - var drain = new FindPairsSweepMethods.FindPairsProcessorDrain(); - //drain.drainBuffer1024 = new NativeArray(1024, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - drain.processor = processor; - - if (index == 0) + var hashSet = new NativeParallelHashSet(layerA.count + layerB.count, Allocator.Temp); + for (int i = 0; i < layerA.count; i++) { - var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); - for (int i = 0; i < layerB.bucketCount - 1; i++) + if (!hashSet.Add(layerA.bodies[i].entity)) { - var bucket = layerB.GetBucketSlices(i); - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, crossBucket, bucket, layerA.bucketCount + i, ref drain); + //Note: At this point, we know the issue lies exclusively in layerA. + var entity = layerA.bodies[i].entity; + throw new InvalidOperationException( + $"A parallel FindPairs job was scheduled using a layer containing more than one instance of Entity {entity}"); } } - else if (index == 1) + for (int i = 0; i < layerB.count; i++) { - var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); - for (int i = 0; i < layerA.bucketCount - 1; i++) + if (!hashSet.Add(layerB.bodies[i].entity)) { - var bucket = layerA.GetBucketSlices(i); - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, bucket, crossBucket, layerA.bucketCount + layerB.bucketCount + i, ref drain); + //Note: At this point, it is unknown whether the repeating entity first showed up in layerA or layerB. + var entity = layerB.bodies[i].entity; + throw new InvalidOperationException( + $"A parallel FindPairs job was scheduled using two layers combined containing more than one instance of Entity {entity}"); } } } - } + #endregion - [BurstCompile] - public struct LayerLayerParallelUnsafe : IJobFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; - - public void Execute(int i) + [Preserve] + void RequireEarlyJobInit() { - var drain = new FindPairsSweepMethods.FindPairsProcessorDrain(); - //drain.drainBuffer1024 = new NativeArray(1024, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - drain.processor = processor; - - if (i < layerA.bucketCount) - { - var bucketA = layerA.GetBucketSlices(i); - var bucketB = layerB.GetBucketSlices(i); - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, bucketA, bucketB, i, ref drain); - } - else if (i < 2 * layerB.bucketCount - 1) - { - i -= layerB.bucketCount; - var bucket = layerB.GetBucketSlices(i); - var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, crossBucket, bucket, i + layerB.bucketCount, ref drain); - } - else - { - var jobIndex = i; - i -= (2 * layerB.bucketCount - 1); - var bucket = layerA.GetBucketSlices(i); - var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, bucket, crossBucket, jobIndex, ref drain); - } + new InitJobsForProcessors.FindPairsIniter().Init(); } } - #endregion - - #region ImmediateMethods - public static void RunImmediate(CollisionLayer layerA, CollisionLayer layerB, T processor) + public static void RunImmediate(in CollisionLayer layerA, in CollisionLayer layerB, ref T processor, bool isThreadSafe) { - var drain = new FindPairsSweepMethods.FindPairsProcessorDrain(); - //drain.drainBuffer1024 = new NativeArray(1024, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - drain.processor = processor; - int jobIndex = 0; for (int i = 0; i < layerA.bucketCount; i++) { var bucketA = layerA.GetBucketSlices(i); var bucketB = layerB.GetBucketSlices(i); - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, bucketA, bucketB, jobIndex++, ref drain, false); + var context = new FindPairsBucketContext(in layerA, + in layerB, + in bucketA, + in bucketB, + jobIndex, + isThreadSafe, + isThreadSafe, + !isThreadSafe); + if (processor.BeginBucket(in context)) + { + if (i != layerA.bucketCount - 1) + FindPairsSweepMethods.BipartiteSweepCellCell(in layerA, + in layerB, + in bucketA, + in bucketB, + jobIndex, + ref processor, + isThreadSafe, + isThreadSafe, + !isThreadSafe); + else + FindPairsSweepMethods.BipartiteSweepCrossCross(in layerA, + in layerB, + in bucketA, + in bucketB, + jobIndex, + ref processor, + isThreadSafe, + isThreadSafe, + !isThreadSafe); + processor.EndBucket(in context); + } + jobIndex++; } var crossBucketA = layerA.GetBucketSlices(layerA.bucketCount - 1); for (int i = 0; i < layerA.bucketCount - 1; i++) { - var bucket = layerB.GetBucketSlices(i); - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, crossBucketA, bucket, jobIndex++, ref drain, false); + var bucket = layerB.GetBucketSlices(i); + var context = new FindPairsBucketContext(in layerA, + in layerB, + in crossBucketA, + in bucket, + jobIndex, + isThreadSafe, + isThreadSafe, + !isThreadSafe); + if (processor.BeginBucket(in context)) + { + FindPairsSweepMethods.BipartiteSweepCrossCell(in layerA, + in layerB, + in crossBucketA, + in bucket, + jobIndex, + ref processor, + isThreadSafe, + isThreadSafe, + !isThreadSafe); + processor.EndBucket(in context); + } + jobIndex++; } var crossBucketB = layerB.GetBucketSlices(layerB.bucketCount - 1); for (int i = 0; i < layerA.bucketCount - 1; i++) { - var bucket = layerA.GetBucketSlices(i); - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, bucket, crossBucketB, jobIndex++, ref drain, false); + var bucket = layerA.GetBucketSlices(i); + var context = new FindPairsBucketContext(in layerA, + in layerB, + in bucket, + in crossBucketB, + jobIndex, + isThreadSafe, + isThreadSafe, + !isThreadSafe); + if (processor.BeginBucket(in context)) + { + FindPairsSweepMethods.BipartiteSweepCellCross(in layerA, + in layerB, + in bucket, + in crossBucketB, + jobIndex, + ref processor, + isThreadSafe, + isThreadSafe, + !isThreadSafe); + processor.EndBucket(in context); + } + jobIndex++; } } - #endregion - - #region SafeChecks - -#if ENABLE_UNITY_COLLECTIONS_CHECKS - - [BurstCompile] - public struct LayerLayerPart2_WithSafety : IJobParallelFor - { - [ReadOnly] public CollisionLayer layerA; - [ReadOnly] public CollisionLayer layerB; - public T processor; - - public void Execute(int index) - { - var drain = new FindPairsSweepMethods.FindPairsProcessorDrain(); - //drain.drainBuffer1024 = new NativeArray(1024, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - drain.processor = processor; + } + } - //var marker0 = new Unity.Profiling.ProfilerMarker("Cross A Unrolled"); - //var marker1 = new Unity.Profiling.ProfilerMarker("Cross B Unrolled"); + [BurstCompile] + internal struct FindPairsParallelByACrossCacheJob : IJobFor + { + [ReadOnly] public CollisionLayer layerA; + [ReadOnly] public CollisionLayer layerB; + public UnsafeIndexedBlockList cache; - if (index == 0) - { - var crossBucket = layerA.GetBucketSlices(layerA.bucketCount - 1); - for (int i = 0; i < layerB.bucketCount - 1; i++) - { - var bucket = layerB.GetBucketSlices(i); - //marker0.Begin(); - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, crossBucket, bucket, layerA.bucketCount + i, ref drain); - //marker0.End(); - } - } - else if (index == 1) - { - var crossBucket = layerB.GetBucketSlices(layerB.bucketCount - 1); - for (int i = 0; i < layerA.bucketCount - 1; i++) - { - var bucket = layerA.GetBucketSlices(i); - //var marker1Detailed = new Unity.Profiling.ProfilerMarker($"Cross B Unrolled {crossBucket.count} - {bucket.count}"); - //marker1Detailed.Begin(); - //marker1.Begin(); - FindPairsSweepMethods.BipartiteSweepUnrolled(layerA, layerB, bucket, crossBucket, layerA.bucketCount + layerB.bucketCount + i, ref drain); - //marker1.End(); - //marker1Detailed.End(); - } - } - else - { - EntityAliasCheck(layerA, layerB); - } + public void Execute(int index) + { + var a = layerA.GetBucketSlices(layerA.bucketCount - 1); + var b = layerB.GetBucketSlices(index); + var cacher = new Cacher { cache = cache, writeIndex = index }; + FindPairsSweepMethods.BipartiteSweepCrossCell(in layerA, in layerB, in a, in b, index, ref cacher, false, false); + } - //if (drain.maxCount > 1000) - // UnityEngine.Debug.Log($"hit count: {drain.maxCount}"); - } - } + struct Cacher : IFindPairsProcessor + { + public UnsafeIndexedBlockList cache; + public int writeIndex; - [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] - private static void EntityAliasCheck(CollisionLayer layerA, CollisionLayer layerB) + public void Execute(in FindPairsResult result) { - var hashSet = new NativeParallelHashSet(layerA.count + layerB.count, Allocator.Temp); - for (int i = 0; i < layerA.count; i++) - { - if (!hashSet.Add(layerA.bodies[i].entity)) - { - //Note: At this point, we know the issue lies exclusively in layerA. - var entity = layerA.bodies[i].entity; - throw new InvalidOperationException( - $"A parallel FindPairs job was scheduled using a layer containing more than one instance of Entity {entity}"); - } - } - for (int i = 0; i < layerB.count; i++) - { - if (!hashSet.Add(layerB.bodies[i].entity)) - { - //Note: At this point, it is unknown whether the repeating entity first showed up in layerA or layerB. - var entity = layerB.bodies[i].entity; - throw new InvalidOperationException( - $"A parallel FindPairs job was scheduled using two layers combined containing more than one instance of Entity {entity}"); - } - } + cache.Write(new int2(result.bodyIndexA, result.bodyIndexB), writeIndex); } -#endif - - #endregion } } } diff --git a/PsyshockPhysics/Physics/Internal/Queries/Layers/FindPairsSweepMethods.cs b/PsyshockPhysics/Physics/Internal/Queries/Layers/FindPairsSweepMethods.cs index 1be94bc..8b40005 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/Layers/FindPairsSweepMethods.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/Layers/FindPairsSweepMethods.cs @@ -1,4 +1,5 @@ -using Unity.Burst; +using Latios.Unsafe; +using Unity.Burst; using Unity.Burst.CompilerServices; using Unity.Burst.Intrinsics; using Unity.Collections; @@ -9,22 +10,191 @@ namespace Latios.Psyshock { internal static class FindPairsSweepMethods { - #region Production Sweeps - public static void SelfSweep(in CollisionLayer layer, in BucketSlices bucket, int jobIndex, ref T processor, bool isThreadSafe = true) where T : struct, - IFindPairsProcessor + #region Dispatchers + public static void SelfSweepCell(in CollisionLayer layer, + in BucketSlices bucket, + int jobIndex, + ref T processor, + bool isAThreadSafe, + bool isBThreadSafe, + bool isImmediateContext = false) where T : struct, IFindPairsProcessor + { + if (bucket.count < 2) + return; + + var result = new FindPairsResult(in layer, in layer, in bucket, in bucket, jobIndex, isAThreadSafe, isBThreadSafe, isImmediateContext); + + if (X86.Avx.IsAvxSupported) + { + SelfSweepWholeBucketAvx(ref result, bucket, ref processor); + } + else + { + SelfSweepWholeBucket(ref result, bucket, ref processor); + } + } + + public static void SelfSweepCross(in CollisionLayer layer, + in BucketSlices bucket, + int jobIndex, + ref T processor, + bool isAThreadSafe, + bool isBThreadSafe, + bool isImmediateContext = false) where T : struct, IFindPairsProcessor + { + SelfSweepCell(in layer, in bucket, jobIndex, ref processor, isAThreadSafe, isBThreadSafe, isImmediateContext); + } + + public static void BipartiteSweepCellCell(in CollisionLayer layerA, + in CollisionLayer layerB, + in BucketSlices bucketA, + in BucketSlices bucketB, + int jobIndex, + ref T processor, + bool isAThreadSafe, + bool isBThreadSafe, + bool isImmediateContext = false) where T : struct, IFindPairsProcessor { + int countA = bucketA.xmins.Length; + int countB = bucketB.xmins.Length; + if (countA == 0 || countB == 0) + return; + + var result = new FindPairsResult(in layerA, in layerB, in bucketA, in bucketB, jobIndex, isAThreadSafe, isBThreadSafe, isImmediateContext); + if (X86.Avx.IsAvxSupported) { - SelfSweepAvx(layer, bucket, jobIndex, ref processor, isThreadSafe); + BipartiteSweepWholeBucketAvx(ref result, in bucketA, in bucketB, ref processor); + } + else + { + BipartiteSweepWholeBucket(ref result, in bucketA, in bucketB, ref processor); + } + } + + public static void BipartiteSweepCellCross(in CollisionLayer layerA, + in CollisionLayer layerB, + in BucketSlices bucketA, + in BucketSlices bucketB, + int jobIndex, + ref T processor, + bool isAThreadSafe, + bool isBThreadSafe, + bool isImmediateContext = false) where T : struct, IFindPairsProcessor + { + int countA = bucketA.xmins.Length; + int countB = bucketB.xmins.Length; + if (countA == 0 || countB == 0) return; + + var result = new FindPairsResult(in layerA, in layerB, in bucketA, in bucketB, jobIndex, isAThreadSafe, isBThreadSafe, isImmediateContext); + + if (bucketB.count < 32) + BipartiteSweepWholeBucket(ref result, in bucketA, in bucketB, ref processor); + else + BipartiteSweepBucketVsFilteredCross(ref result, in bucketA, in bucketB, ref processor, new BucketAabb(in layerA, bucketA.bucketIndex)); + } + + public static void BipartiteSweepCrossCell(in CollisionLayer layerA, + in CollisionLayer layerB, + in BucketSlices bucketA, + in BucketSlices bucketB, + int jobIndex, + ref T processor, + bool isAThreadSafe, + bool isBThreadSafe, + bool isImmediateContext = false) where T : struct, IFindPairsProcessor + { + int countA = bucketA.xmins.Length; + int countB = bucketB.xmins.Length; + if (countA == 0 || countB == 0) + return; + + var result = new FindPairsResult(in layerA, in layerB, in bucketA, in bucketB, jobIndex, isAThreadSafe, isBThreadSafe, isImmediateContext); + + if (bucketA.count < 32) + BipartiteSweepWholeBucket(ref result, in bucketA, in bucketB, ref processor); + else + BipartiteSweepFilteredCrossVsBucket(ref result, in bucketA, in bucketB, ref processor, new BucketAabb(in layerB, bucketB.bucketIndex)); + } + + public static void BipartiteSweepCrossCross(in CollisionLayer layerA, + in CollisionLayer layerB, + in BucketSlices bucketA, + in BucketSlices bucketB, + int jobIndex, + ref T processor, + bool isAThreadSafe, + bool isBThreadSafe, + bool isImmediateContext = false) where T : struct, IFindPairsProcessor + { + BipartiteSweepCellCell(in layerA, in layerB, in bucketA, in bucketB, jobIndex, ref processor, isAThreadSafe, isBThreadSafe, isImmediateContext); + } + + public static int BipartiteSweepPlayCache(UnsafeIndexedBlockList.Enumerator enumerator, + in CollisionLayer layerA, + in CollisionLayer layerB, + int bucketIndexA, + int bucketIndexB, + int jobIndex, + ref T processor, + bool isAThreadSafe, + bool isBThreadSafe) where T : struct, IFindPairsProcessor + { + if (!enumerator.MoveNext()) + return 0; + + var result = FindPairsResult.CreateGlobalResult(in layerA, in layerB, bucketIndexA, bucketIndexB, jobIndex, isAThreadSafe, isBThreadSafe); + int count = 0; + + do + { + var indices = enumerator.GetCurrent(); + result.SetBucketRelativePairIndices(indices.x, indices.y); + processor.Execute(in result); + count++; + } + while (enumerator.MoveNext()); + return count; + } + #endregion + + #region Utilities + struct BucketAabb + { + public float xmin; + public float xmax; + public float4 yzMinMaxFlipped; + public bool4 finiteMask; + + public BucketAabb(in CollisionLayer layer, int bucketIndex) + { + var dimensions = layer.worldSubdivisionsPerAxis; + int k = bucketIndex % dimensions.z; + int j = ((bucketIndex - k) / dimensions.z) % dimensions.y; + int i = (((bucketIndex - k) / dimensions.z) - j) / dimensions.y; + var bucketStart = layer.worldMin + layer.worldAxisStride * new float3(i, j, k); + var bucketEnd = bucketStart + layer.worldAxisStride; + xmin = math.select(float.NegativeInfinity, bucketStart.x, i > 0); + xmax = math.select(float.PositiveInfinity, bucketEnd.x, i < dimensions.x - 1); + yzMinMaxFlipped = new float4(bucketStart.yz, -bucketEnd.yz); + yzMinMaxFlipped = -yzMinMaxFlipped.zwxy; + bool a = j < dimensions.y - 1; // cell.max.y < AABB.min.y + bool b = k < dimensions.z - 1; // cell.max.z < AABB.min.z + bool c = j > 0; // -cell.min.y < -AABB.max.y + bool d = k > 0; // -cell.min.z < -AABB.max.z + finiteMask = new bool4(a, b, c, d); } + } + #endregion + #region Self Sweeps + static void SelfSweepWholeBucket(ref FindPairsResult result, in BucketSlices bucket, ref T processor) where T : struct, IFindPairsProcessor + { Hint.Assume(bucket.xmins.Length == bucket.xmaxs.Length); Hint.Assume(bucket.xmins.Length == bucket.yzminmaxs.Length); Hint.Assume(bucket.xmins.Length == bucket.bodies.Length); - var result = new FindPairsResult(in layer, in layer, in bucket, in bucket, jobIndex, isThreadSafe); - int count = bucket.xmins.Length; for (int i = 0; i < count - 1; i++) { @@ -41,7 +211,7 @@ public static void SelfSweep(in CollisionLayer layer, in BucketSlices bucket, } } - static unsafe void SelfSweepAvx(in CollisionLayer layer, in BucketSlices bucket, int jobIndex, ref T processor, bool isThreadSafe = true) where T : struct, + static unsafe void SelfSweepWholeBucketAvx(ref FindPairsResult result, in BucketSlices bucket, ref T processor) where T : struct, IFindPairsProcessor { if (X86.Avx.IsAvxSupported) @@ -50,8 +220,6 @@ static unsafe void SelfSweepAvx(in CollisionLayer layer, in BucketSlices buck Hint.Assume(bucket.xmins.Length == bucket.yzminmaxs.Length); Hint.Assume(bucket.xmins.Length == bucket.bodies.Length); - var result = new FindPairsResult(in layer, in layer, in bucket, in bucket, jobIndex, isThreadSafe); - int count = bucket.xmins.Length; for (int i = 0; i < count - 1; i++) { @@ -90,27 +258,15 @@ static unsafe void SelfSweepAvx(in CollisionLayer layer, in BucketSlices buck } } } + #endregion - public static void BipartiteSweep(in CollisionLayer layerA, - in CollisionLayer layerB, - in BucketSlices bucketA, - in BucketSlices bucketB, - int jobIndex, - ref T processor, - bool isThreadSafe = true) where T : struct, + #region Bipartite Sweeps + static unsafe void BipartiteSweepWholeBucket(ref FindPairsResult result, + in BucketSlices bucketA, + in BucketSlices bucketB, + ref T processor) where T : struct, IFindPairsProcessor { - if (X86.Avx.IsAvxSupported) - { - BipartiteSweepAvx(in layerA, in layerB, in bucketA, in bucketB, jobIndex, ref processor, isThreadSafe); - return; - } - - int countA = bucketA.xmins.Length; - int countB = bucketB.xmins.Length; - if (countA == 0 || countB == 0) - return; - Hint.Assume(bucketA.xmins.Length == bucketA.xmaxs.Length); Hint.Assume(bucketA.xmins.Length == bucketA.yzminmaxs.Length); Hint.Assume(bucketA.xmins.Length == bucketA.bodies.Length); @@ -119,7 +275,8 @@ public static void BipartiteSweep(in CollisionLayer layerA, Hint.Assume(bucketB.xmins.Length == bucketB.yzminmaxs.Length); Hint.Assume(bucketB.xmins.Length == bucketB.bodies.Length); - var result = new FindPairsResult(in layerA, in layerB, in bucketA, in bucketB, jobIndex, isThreadSafe); + int countA = bucketA.xmins.Length; + int countB = bucketB.xmins.Length; //Check for b starting in a's x range int bstart = 0; @@ -168,22 +325,14 @@ public static void BipartiteSweep(in CollisionLayer layerA, } } - static unsafe void BipartiteSweepAvx(in CollisionLayer layerA, - in CollisionLayer layerB, - in BucketSlices bucketA, - in BucketSlices bucketB, - int jobIndex, - ref T processor, - bool isThreadSafe = true) where T : struct, + static unsafe void BipartiteSweepWholeBucketAvx(ref FindPairsResult result, + in BucketSlices bucketA, + in BucketSlices bucketB, + ref T processor) where T : struct, IFindPairsProcessor { if (X86.Avx.IsAvxSupported) { - int countA = bucketA.xmins.Length; - int countB = bucketB.xmins.Length; - if (countA == 0 || countB == 0) - return; - Hint.Assume(bucketA.xmins.Length == bucketA.xmaxs.Length); Hint.Assume(bucketA.xmins.Length == bucketA.yzminmaxs.Length); Hint.Assume(bucketA.xmins.Length == bucketA.bodies.Length); @@ -192,7 +341,8 @@ static unsafe void BipartiteSweepAvx(in CollisionLayer layerA, Hint.Assume(bucketB.xmins.Length == bucketB.yzminmaxs.Length); Hint.Assume(bucketB.xmins.Length == bucketB.bodies.Length); - var result = new FindPairsResult(in layerA, in layerB, in bucketA, in bucketB, jobIndex, isThreadSafe); + int countA = bucketA.xmins.Length; + int countB = bucketB.xmins.Length; //Check for b starting in a's x range int bstart = 0; @@ -283,85 +433,13 @@ static unsafe void BipartiteSweepAvx(in CollisionLayer layerA, } } } - #endregion - - #region Sweep Stats - public static void SelfSweepStats(BucketSlices bucket, in FixedString128Bytes layerName) - { - int hitCount = 0; - int innerLoopEnterCount = 0; - int innerLoopTestCount = 0; - int innerLoopRunMin = int.MaxValue; - int innerLoopRunMax = 0; - int innerLoopZHits = 0; - - Hint.Assume(bucket.xmins.Length == bucket.xmaxs.Length); - Hint.Assume(bucket.xmins.Length == bucket.yzminmaxs.Length); - Hint.Assume(bucket.xmins.Length == bucket.bodies.Length); - - int count = bucket.xmins.Length; - for (int i = 0; i < count - 1; i++) - { - int runCount = 0; - var current = -bucket.yzminmaxs[i].zwxy; - for (int j = i + 1; j < count && bucket.xmins[j] <= bucket.xmaxs[i]; j++) - { - runCount++; - //float4 less = math.shuffle(current, - // bucket.yzminmaxs[j], - // math.ShuffleComponent.RightZ, - // math.ShuffleComponent.RightW, - // math.ShuffleComponent.LeftZ, - // math.ShuffleComponent.LeftW - // ); - //float4 more = math.shuffle(current, - // bucket.yzminmaxs[j], - // math.ShuffleComponent.LeftX, - // math.ShuffleComponent.LeftY, - // math.ShuffleComponent.RightX, - // math.ShuffleComponent.RightY - // ); - - if (math.bitmask(current < bucket.yzminmaxs[j]) == 0) - { - hitCount++; - } - if ((math.bitmask(current < bucket.yzminmaxs[j]) & 0xa) == 0) - innerLoopZHits++; - //if (less.y >= more.y && less.w >= more.w) - // innerLoopZHits++; - } - if (runCount > 0) - innerLoopEnterCount++; - innerLoopTestCount += runCount; - innerLoopRunMax = math.max(innerLoopRunMax, runCount); - innerLoopRunMin = math.min(innerLoopRunMin, runCount); - } - - //SelfSweepDualGenAndStats(bucket, in layerName); - UnityEngine.Debug.Log( - $"FindPairs Self Sweep stats for layer {layerName} at bucket index {bucket.bucketIndex} and count {bucket.count}\nHits: {hitCount}, inner loop enters: {innerLoopEnterCount}, inner loop tests: {innerLoopTestCount}, inner loop run (min, max): ({innerLoopRunMin}, {innerLoopRunMax}), inner loop z hits: {innerLoopZHits}"); - } - public static void BipartiteSweepStats(BucketSlices bucketA, in FixedString128Bytes layerNameA, BucketSlices bucketB, in FixedString128Bytes layerNameB) + static unsafe void BipartiteSweepBucketVsFilteredCross(ref FindPairsResult result, + in BucketSlices bucketA, + in BucketSlices bucketB, + ref T processor, + in BucketAabb bucketAabbForA) where T : struct, IFindPairsProcessor { - int hitCountA = 0; - int innerLoopEnterCountA = 0; - int innerLoopTestCountA = 0; - int innerLoopRunMinA = int.MaxValue; - int innerLoopRunMaxA = 0; - - int hitCountB = 0; - int innerLoopEnterCountB = 0; - int innerLoopTestCountB = 0; - int innerLoopRunMinB = int.MaxValue; - int innerLoopRunMaxB = 0; - - int countA = bucketA.xmins.Length; - int countB = bucketB.xmins.Length; - if (countA == 0 || countB == 0) - return; - Hint.Assume(bucketA.xmins.Length == bucketA.xmaxs.Length); Hint.Assume(bucketA.xmins.Length == bucketA.yzminmaxs.Length); Hint.Assume(bucketA.xmins.Length == bucketA.bodies.Length); @@ -370,1462 +448,169 @@ public static void BipartiteSweepStats(BucketSlices bucketA, in FixedString128By Hint.Assume(bucketB.xmins.Length == bucketB.yzminmaxs.Length); Hint.Assume(bucketB.xmins.Length == bucketB.bodies.Length); - //Check for b starting in a's x range + int countA = bucketA.xmins.Length; + int countB = bucketB.xmins.Length; + + using var allocator = ThreadStackAllocator.GetAllocator(); + + var crossXMins = allocator.Allocate(countB + 1); + var crossXMaxs = allocator.Allocate(countB); + var crossYzMinMaxs = allocator.Allocate(countB); + var crossIndices = allocator.Allocate(countB); + int crossCount = 0; + + for (int i = 0; i < countB; i++) + { + if (bucketAabbForA.xmax < bucketB.xmins[i]) + break; + if (bucketB.xmaxs[i] < bucketAabbForA.xmin) + continue; + if (math.bitmask((bucketAabbForA.yzMinMaxFlipped < bucketB.yzminmaxs[i]) & bucketAabbForA.finiteMask) == 0) + { + crossXMins[crossCount] = bucketB.xmins[i]; + crossXMaxs[crossCount] = bucketB.xmaxs[i]; + crossYzMinMaxs[crossCount] = bucketB.yzminmaxs[i]; + crossIndices[crossCount] = i; + crossCount++; + } + } + crossXMins[crossCount] = float.NaN; + //UnityEngine.Debug.Log($"Remaining after filter: {crossCount * 100f / countB}"); + + // Check for b starting in a's x range int bstart = 0; for (int i = 0; i < countA; i++) { - //Advance to b.xmin >= a.xmin - //Include equals case by stopping when equal - while (bstart < countB && bucketB.xmins[bstart] < bucketA.xmins[i]) + // Advance to b.xmin >= a.xmin + // Include equals case by stopping when equal + while (bstart < crossCount && crossXMins[bstart] < bucketA.xmins[i]) bstart++; - if (bstart >= countB) + if (bstart >= crossCount) break; - int runCount = 0; - var current = -bucketA.yzminmaxs[i].zwxy; - for (int j = bstart; j < countB && bucketB.xmins[j] <= bucketA.xmaxs[i]; j++) + var current = -bucketA.yzminmaxs[i].zwxy; + var xmax = bucketA.xmaxs[i]; + for (int j = bstart; j < crossCount && crossXMins[j] <= xmax; j++) { - runCount++; - - if (math.bitmask(current < bucketB.yzminmaxs[j]) == 0) + if (math.bitmask(current < crossYzMinMaxs[j]) == 0) { - hitCountA++; + result.SetBucketRelativePairIndices(i, crossIndices[j]); + processor.Execute(in result); } } - if (runCount > 0) - innerLoopEnterCountA++; - innerLoopTestCountA += runCount; - innerLoopRunMaxA = math.max(innerLoopRunMaxA, runCount); - innerLoopRunMinA = math.min(innerLoopRunMinA, runCount); } - //Check for a starting in b's x range + // Check for a starting in b's x range int astart = 0; - for (int i = 0; i < countB; i++) + for (int i = 0; i < crossCount; i++) { - //Advance to a.xmin > b.xmin - //Exclude equals case this time by continuing if equal - while (astart < countA && bucketA.xmins[astart] <= bucketB.xmins[i]) + // Advance to a.xmin > b.xmin + // Exclude equals case this time by continuing if equal + while (astart < countA && bucketA.xmins[astart] <= crossXMins[i]) astart++; if (astart >= countA) break; - int runCount = 0; - var current = -bucketB.yzminmaxs[i].zwxy; - for (int j = astart; j < countA && bucketA.xmins[j] <= bucketB.xmaxs[i]; j++) + var current = -crossYzMinMaxs[i].zwxy; + var xmax = crossXMaxs[i]; + for (int j = astart; j < countA && bucketA.xmins[j] <= xmax; j++) { - runCount++; - if (math.bitmask(current < bucketA.yzminmaxs[j]) == 0) { - hitCountB++; + result.SetBucketRelativePairIndices(j, crossIndices[i]); + processor.Execute(in result); } } - if (runCount > 0) - innerLoopEnterCountB++; - innerLoopTestCountB += runCount; - innerLoopRunMaxB = math.max(innerLoopRunMaxB, runCount); - innerLoopRunMinB = math.min(innerLoopRunMinB, runCount); } - - UnityEngine.Debug.Log( - $"FindPairs Bipartite Sweep stats for layerA {layerNameA} at bucket index {bucketA.bucketIndex} and count {bucketA.count}; and layerB {layerNameB} at bucket index {bucketB.bucketIndex} and count {bucketB.count}\n::A SWEEP B:: Hits: {hitCountA}, inner loop enters: {innerLoopEnterCountA}, inner loop tests: {innerLoopTestCountA}, inner loop run (min, max): ({innerLoopRunMinA}, {innerLoopRunMaxA})\n::B SWEEP A:: Hits: {hitCountB}, inner loop enters: {innerLoopEnterCountB}, inner loop tests: {innerLoopTestCountB}, inner loop run (min, max): ({innerLoopRunMinB}, {innerLoopRunMaxB})"); - } - #endregion - - #region Experimental Sweeps - public unsafe interface IFindPairsDrainable - { - public void SetBuckets(CollisionLayer layerA, CollisionLayer layerB, BucketSlices bucketA, BucketSlices bucketB, int jobIndex, bool isThreadSafe); - public ulong* AcquireDrainBuffer1024ForWrite(); - public void Drain(ulong count); - public void DrainStats(ulong count); - public void DirectInvoke(int a, int b); } - public unsafe struct FindPairsProcessorDrain : IFindPairsDrainable where T : struct, IFindPairsProcessor + static unsafe void BipartiteSweepFilteredCrossVsBucket(ref FindPairsResult result, + in BucketSlices bucketA, + in BucketSlices bucketB, + ref T processor, + in BucketAabb bucketAabbForB) where T : struct, IFindPairsProcessor { - public T processor; - - // Packed pairs. If [63:32] is negative, b[63:32], a[31:0] otherwise a[63:32], b[31:0] - //public NativeArray drainBuffer1024; - fixed ulong drainBuffer1024[1024]; - FindPairsResult m_result; - ulong m_combinedCount; + Hint.Assume(bucketA.xmins.Length == bucketA.xmaxs.Length); + Hint.Assume(bucketA.xmins.Length == bucketA.yzminmaxs.Length); + Hint.Assume(bucketA.xmins.Length == bucketA.bodies.Length); - public ulong maxCount; + Hint.Assume(bucketB.xmins.Length == bucketB.xmaxs.Length); + Hint.Assume(bucketB.xmins.Length == bucketB.yzminmaxs.Length); + Hint.Assume(bucketB.xmins.Length == bucketB.bodies.Length); - public void SetBuckets(CollisionLayer layerA, CollisionLayer layerB, BucketSlices bucketA, BucketSlices bucketB, int jobIndex, bool isThreadSafe) - { - m_result = new FindPairsResult(layerA, layerB, bucketA, bucketB, jobIndex, isThreadSafe); - m_combinedCount = (ulong)bucketA.count + (ulong)bucketB.count; - } + int countA = bucketA.xmins.Length; + int countB = bucketB.xmins.Length; - public ulong* AcquireDrainBuffer1024ForWrite() - { - fixed (ulong* ptr = drainBuffer1024) - return ptr; - //return (ulong*)drainBuffer1024.GetUnsafePtr(); - } + using var allocator = ThreadStackAllocator.GetAllocator(); - public void DirectInvoke(int a, int b) - { - m_result.SetBucketRelativePairIndices(a, b); - processor.Execute(in m_result); - } + var crossXMins = allocator.Allocate(countA + 1); + var crossXMaxs = allocator.Allocate(countA); + var crossYzMinMaxs = allocator.Allocate(countA); + var crossIndices = allocator.Allocate(countA); + int crossCount = 0; - public void Drain(ulong count) + for (int i = 0; i < countA; i++) { - maxCount += count; - //var marker = new Unity.Profiling.ProfilerMarker("Drain"); - - //marker.Begin(); - - for (ulong i = 0; i < count; i++) + if (bucketAabbForB.xmax < bucketA.xmins[i]) + break; + if (bucketA.xmaxs[i] < bucketAabbForB.xmin) + continue; + if (math.bitmask((bucketAabbForB.yzMinMaxFlipped < bucketA.yzminmaxs[i]) & bucketAabbForB.finiteMask) == 0) { - var pair = drainBuffer1024[(int)i]; - ulong shiftedA = pair >> 32; - - int b = (int)(pair & 0xffffffff); - - if (shiftedA >= m_combinedCount) - { - int a = (int)(shiftedA - m_combinedCount); - m_result.SetBucketRelativePairIndices(b, a); - processor.Execute(in m_result); - } - else - { - int a = (int)shiftedA; - m_result.SetBucketRelativePairIndices(a, b); - processor.Execute(in m_result); - } + crossXMins[crossCount] = bucketA.xmins[i]; + crossXMaxs[crossCount] = bucketA.xmaxs[i]; + crossYzMinMaxs[crossCount] = bucketA.yzminmaxs[i]; + crossIndices[crossCount] = i; + crossCount++; } - - //marker.End(); } + crossXMins[crossCount] = float.NaN; + //UnityEngine.Debug.Log($"Remaining after filter: {crossCount * 100f / countA}"); - public void DrainStats(ulong count) + // Check for b starting in a's x range + int bstart = 0; + for (int i = 0; i < crossCount; i++) { - maxCount += count; - - //var processNormal = new Unity.Profiling.ProfilerMarker("Normal"); - //var processFlipped = new Unity.Profiling.ProfilerMarker("Flipped"); + // Advance to b.xmin >= a.xmin + // Include equals case by stopping when equal + while (bstart < countB && bucketB.xmins[bstart] < crossXMins[i]) + bstart++; + if (bstart >= countB) + break; - for (ulong i = 0; i < count; i++) + var current = -crossYzMinMaxs[i].zwxy; + var xmax = crossXMaxs[i]; + for (int j = bstart; j < countB && bucketB.xmins[j] <= xmax; j++) { - var pair = drainBuffer1024[(int)i]; - ulong shiftedA = pair >> 32; - - int b = (int)(pair & 0xffffffff); - - if (shiftedA >= m_combinedCount) - { - int a = (int)(shiftedA - m_combinedCount); - - //processFlipped.Begin(); - m_result.SetBucketRelativePairIndices(b, a); - processor.Execute(in m_result); - //processFlipped.End(); - } - else + if (math.bitmask(current < bucketB.yzminmaxs[j]) == 0) { - int a = (int)shiftedA; - m_result.SetBucketRelativePairIndices(a, b); - //processNormal.Begin(); - processor.Execute(in m_result); - //processNormal.End(); + result.SetBucketRelativePairIndices(crossIndices[i], j); + processor.Execute(in result); } } } - } - - public static unsafe void SelfSweepUnrolled(CollisionLayer layer, BucketSlices bucket, int jobIndex, ref T drain, bool isThreadSafe = true) where T : struct, - IFindPairsDrainable - { - Hint.Assume(bucket.xmins.Length == bucket.xmaxs.Length); - Hint.Assume(bucket.xmins.Length == bucket.yzminmaxs.Length); - Hint.Assume(bucket.xmins.Length == bucket.bodies.Length); - - int count = bucket.xmins.Length; - ulong nextHitIndex = 0; - drain.SetBuckets(layer, layer, bucket, bucket, jobIndex, isThreadSafe); - var hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - var minMaxPtr = (float4*)bucket.yzminmaxs.GetUnsafeReadOnlyPtr(); - var xminsPtr = (float*)bucket.xmins.GetUnsafeReadOnlyPtr(); - for (int i = 0; i < bucket.xmins.Length - 1; i++) + // Check for a starting in b's x range + int astart = 0; + for (int i = 0; i < countB; i++) { - float4 current = -minMaxPtr[i].zwxy; - - float currentX = bucket.xmaxs[i]; - - ulong j = (ulong)i + 1; - - ulong pair = (((ulong)i) << 32) | j; - ulong final = (((ulong)i) << 32) | ((uint)bucket.xmins.Length); + // Advance to a.xmin > b.xmin + // Exclude equals case this time by continuing if equal + while (astart < crossCount && crossXMins[astart] <= bucketB.xmins[i]) + astart++; + if (astart >= crossCount) + break; - while (pair + 15 < final) + var current = -bucketB.yzminmaxs[i].zwxy; + var xmax = bucketB.xmaxs[i]; + for (int j = astart; j < crossCount && crossXMins[j] <= xmax; j++) { - if (Hint.Unlikely(xminsPtr[(j + 15)] >= currentX)) - break; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 1]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 2]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 3]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 4]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 5]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 6]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 7]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 8]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 9]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 10]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 11]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 12]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 13]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 14]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j + 15]) == 0) - nextHitIndex++; - pair++; - j += 16; - - if (Hint.Unlikely(nextHitIndex >= 1008)) + if (math.bitmask(current < crossYzMinMaxs[j]) == 0) { - drain.Drain(nextHitIndex); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; + result.SetBucketRelativePairIndices(crossIndices[j], i); + processor.Execute(in result); } } - - while (pair < final && xminsPtr[j] < currentX) - { - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtr[j]) == 0) - nextHitIndex++; - pair++; - j++; - } - - if (nextHitIndex >= 1008) - { - drain.Drain(nextHitIndex); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - if (nextHitIndex > 0) - drain.Drain(nextHitIndex); - } - - public static unsafe void BipartiteSweepUnrolled2(CollisionLayer layerA, - CollisionLayer layerB, - BucketSlices bucketA, - BucketSlices bucketB, - int jobIndex, - ref T drain, - bool isThreadSafe = true) where T : struct, - IFindPairsDrainable - { - int countA = bucketA.xmins.Length; - int countB = bucketB.xmins.Length; - if (countA == 0 || countB == 0) - return; - - Hint.Assume(bucketA.xmins.Length == bucketA.xmaxs.Length); - Hint.Assume(bucketA.xmins.Length == bucketA.yzminmaxs.Length); - Hint.Assume(bucketA.xmins.Length == bucketA.bodies.Length); - - Hint.Assume(bucketB.xmins.Length == bucketB.xmaxs.Length); - Hint.Assume(bucketB.xmins.Length == bucketB.yzminmaxs.Length); - Hint.Assume(bucketB.xmins.Length == bucketB.bodies.Length); - - ulong nextHitIndex = 0; - drain.SetBuckets(layerA, layerB, bucketA, bucketB, jobIndex, isThreadSafe); - var hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - var minMaxPtrA = (float4*)bucketA.yzminmaxs.GetUnsafeReadOnlyPtr(); - var minMaxPtrB = (float4*)bucketB.yzminmaxs.GetUnsafeReadOnlyPtr(); - - //ulong tests = 0; - - //Check for b starting in a's x range - int bstart = 0; - for (int i = 0; i < countA; i++) - { - //Advance to b.xmin >= a.xmin - //Include equals case by stopping when equal - while (bstart < countB && bucketB.xmins[bstart] < bucketA.xmins[i]) - bstart++; - if (bstart >= countB) - break; - - float4 current = -minMaxPtrA[i].zwxy; - - float currentX = bucketA.xmaxs[i]; - - ulong j = (ulong)bstart; - - ulong pair = (((ulong)i) << 32) | j; - ulong final = (((ulong)i) << 32) | ((uint)countB); - - while (pair + 3 < final) - { - if (Hint.Unlikely(bucketB.xmins[(int)(j + 3)] >= currentX)) - break; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 1]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 2]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 3]) == 0) - nextHitIndex++; - pair++; - - j += 4; - - //tests += 16; - - if (Hint.Unlikely(nextHitIndex >= 1008)) - { - drain.Drain(nextHitIndex); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - while (pair < final && bucketB.xmins[(int)j] < currentX) - { - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j]) == 0) - nextHitIndex++; - pair++; - j++; - //tests++; - } - - if (nextHitIndex >= 1008) - { - drain.Drain(nextHitIndex); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - //Check for a starting in b's x range - int astart = 0; - for (int i = 0; i < countB; i++) - { - //Advance to a.xmin > b.xmin - //Exclude equals case this time by continuing if equal - while (astart < countA && bucketA.xmins[astart] <= bucketB.xmins[i]) - astart++; - if (astart >= countA) - break; - - float4 current = -minMaxPtrB[i].zwxy; - - float currentX = bucketB.xmaxs[i]; - - ulong j = (ulong)astart; - - ulong bucketsSum = (ulong)bucketA.count + (ulong)bucketB.count; - - ulong pair = ((((ulong)i) + bucketsSum) << 32) | j; - ulong final = ((((ulong)i) + bucketsSum) << 32) | ((uint)countA); - - while (pair + 3 < final) - { - if (Hint.Unlikely(bucketA.xmins[(int)(j + 3)] >= currentX)) - break; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 1]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 2]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 3]) == 0) - nextHitIndex++; - pair++; - - j += 4; - //tests += 16; - - if (Hint.Unlikely(nextHitIndex >= 1008)) - { - drain.Drain(nextHitIndex); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - while (pair < final && bucketA.xmins[(int)j] < currentX) - { - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j]) == 0) - nextHitIndex++; - pair++; - j++; - //tests++; - } - - if (nextHitIndex >= 1008) - { - drain.Drain(nextHitIndex); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - if (nextHitIndex > 0) - drain.Drain(nextHitIndex); - - //if (tests > 10000) - // UnityEngine.Debug.Log($"Unrolled tests: {tests}"); - } - - public static unsafe void BipartiteSweepUnrolled(CollisionLayer layerA, - CollisionLayer layerB, - in BucketSlices bucketA, - in BucketSlices bucketB, - int jobIndex, - ref T drain, - bool isThreadSafe = true) where T : struct, - IFindPairsDrainable - { - int countA = bucketA.xmins.Length; - int countB = bucketB.xmins.Length; - if (countA == 0 || countB == 0) - return; - - Hint.Assume(bucketA.xmins.Length == bucketA.xmaxs.Length); - Hint.Assume(bucketA.xmins.Length == bucketA.yzminmaxs.Length); - Hint.Assume(bucketA.xmins.Length == bucketA.bodies.Length); - - Hint.Assume(bucketB.xmins.Length == bucketB.xmaxs.Length); - Hint.Assume(bucketB.xmins.Length == bucketB.yzminmaxs.Length); - Hint.Assume(bucketB.xmins.Length == bucketB.bodies.Length); - - ulong nextHitIndex = 0; - drain.SetBuckets(layerA, layerB, bucketA, bucketB, jobIndex, isThreadSafe); - var hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - var minMaxPtrA = (float4*)bucketA.yzminmaxs.GetUnsafeReadOnlyPtr(); - var minMaxPtrB = (float4*)bucketB.yzminmaxs.GetUnsafeReadOnlyPtr(); - var xminsPtrA = (float*)bucketA.xmins.GetUnsafeReadOnlyPtr(); - var xminsPtrB = (float*)bucketB.xmins.GetUnsafeReadOnlyPtr(); - - //ulong tests = 0; - - //Check for b starting in a's x range - int bstart = 0; - for (int i = 0; i < countA; i++) - { - //Advance to b.xmin >= a.xmin - //Include equals case by stopping when equal - while (bstart < countB && xminsPtrB[bstart] < xminsPtrA[i]) - bstart++; - if (bstart >= countB) - break; - - float4 current = -minMaxPtrA[i].zwxy; - - float currentX = bucketA.xmaxs[i]; - - ulong j = (ulong)bstart; - - ulong pair = (((ulong)i) << 32) | j; - ulong final = (((ulong)i) << 32) | ((uint)countB); - - while (Hint.Likely(pair + 15 < final)) - { - if (xminsPtrB[(j + 15)] >= currentX) - break; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 1]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 2]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 3]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 4]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 5]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 6]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 7]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 8]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 9]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 10]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 11]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 12]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 13]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 14]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 15]) == 0) - nextHitIndex++; - pair++; - j += 16; - - //tests += 16; - - if (Hint.Unlikely(nextHitIndex >= 1008)) - { - drain.Drain(nextHitIndex); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - Hint.Assume((pair & 0xffffffff00000000) == (final & 0xffffffff00000000)); - Hint.Assume((pair & 0xffffffff) == j); - Hint.Assume(j <= int.MaxValue); - while (Hint.Likely(pair < final && xminsPtrB[j] < currentX)) - { - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j]) == 0) - nextHitIndex++; - pair++; - j++; - //tests++; - } - - if (Hint.Unlikely(nextHitIndex >= 1008)) - { - drain.Drain(nextHitIndex); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - //Check for a starting in b's x range - int astart = 0; - for (int i = 0; i < countB; i++) - { - //Advance to a.xmin > b.xmin - //Exclude equals case this time by continuing if equal - while (astart < countA && xminsPtrA[astart] <= xminsPtrB[i]) - astart++; - if (astart >= countA) - break; - - float4 current = -minMaxPtrB[i].zwxy; - - float currentX = bucketB.xmaxs[i]; - - ulong j = (ulong)astart; - - ulong bucketsSum = (ulong)bucketA.count + (ulong)bucketB.count; - - ulong pair = ((((ulong)i) + bucketsSum) << 32) | j; - ulong final = ((((ulong)i) + bucketsSum) << 32) | ((uint)countA); - - while (Hint.Likely(pair + 15 < final)) - { - if (xminsPtrA[(j + 15)] >= currentX) - break; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 1]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 2]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 3]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 4]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 5]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 6]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 7]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 8]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 9]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 10]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 11]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 12]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 13]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 14]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 15]) == 0) - nextHitIndex++; - pair++; - j += 16; - //tests += 16; - - if (Hint.Unlikely(nextHitIndex >= 1008)) - { - drain.Drain(nextHitIndex); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - Hint.Assume((pair & 0xffffffff00000000) == (final & 0xffffffff00000000)); - Hint.Assume((pair & 0xffffffff) == j); - Hint.Assume(j <= int.MaxValue); - while (Hint.Likely(pair < final && xminsPtrA[j] < currentX)) - { - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j]) == 0) - nextHitIndex++; - pair++; - j++; - //tests++; - } - - if (nextHitIndex >= 1008) - { - drain.Drain(nextHitIndex); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - if (nextHitIndex > 0) - drain.Drain(nextHitIndex); - - //if (tests > 10000) - // UnityEngine.Debug.Log($"Unrolled tests: {tests}"); - } - - public static unsafe void BipartiteSweepUnrolledStats(CollisionLayer layerA, - CollisionLayer layerB, - BucketSlices bucketA, - BucketSlices bucketB, - int jobIndex, - ref T drain, - bool isThreadSafe = true) where T : struct, - IFindPairsDrainable - { - int countA = bucketA.xmins.Length; - int countB = bucketB.xmins.Length; - if (countA == 0 || countB == 0) - return; - - Hint.Assume(bucketA.xmins.Length == bucketA.xmaxs.Length); - Hint.Assume(bucketA.xmins.Length == bucketA.yzminmaxs.Length); - Hint.Assume(bucketA.xmins.Length == bucketA.bodies.Length); - - Hint.Assume(bucketB.xmins.Length == bucketB.xmaxs.Length); - Hint.Assume(bucketB.xmins.Length == bucketB.yzminmaxs.Length); - Hint.Assume(bucketB.xmins.Length == bucketB.bodies.Length); - - ulong nextHitIndex = 0; - drain.SetBuckets(layerA, layerB, bucketA, bucketB, jobIndex, isThreadSafe); - var hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - var minMaxPtrA = (float4*)bucketA.yzminmaxs.GetUnsafeReadOnlyPtr(); - var minMaxPtrB = (float4*)bucketB.yzminmaxs.GetUnsafeReadOnlyPtr(); - - //ulong tests = 0; - var inner0Marker = new Unity.Profiling.ProfilerMarker("Inner 0"); - var inner1Marker = new Unity.Profiling.ProfilerMarker("Inner 1"); - var lead0Marker = new Unity.Profiling.ProfilerMarker("Lead 0"); - var lead1Marker = new Unity.Profiling.ProfilerMarker("Lead 1"); - var cleanup0Marker = new Unity.Profiling.ProfilerMarker("Cleanup 0"); - var cleanup1Marker = new Unity.Profiling.ProfilerMarker("Cleanup 1"); - var drain0Marker = new Unity.Profiling.ProfilerMarker("Drain 0"); - var drain1Marker = new Unity.Profiling.ProfilerMarker("Drain 1"); - var outer0Marker = new Unity.Profiling.ProfilerMarker("Outer 0"); - var outer1Marker = new Unity.Profiling.ProfilerMarker("Outer 1"); - - outer0Marker.Begin(); - //Check for b starting in a's x range - int bstart = 0; - for (int i = 0; i < countA; i++) - { - //Advance to b.xmin >= a.xmin - //Include equals case by stopping when equal - //lead0Marker.Begin(); - while (bstart < countB && bucketB.xmins[bstart] < bucketA.xmins[i]) - bstart++; - //lead0Marker.End(); - if (bstart >= countB) - break; - - float4 current = -minMaxPtrA[i].zwxy; - - float currentX = bucketA.xmaxs[i]; - - ulong j = (ulong)bstart; - - ulong pair = (((ulong)i) << 32) | j; - ulong final = (((ulong)i) << 32) | ((uint)countB); - - //inner0Marker.Begin(); - while (pair + 15 < final) - { - if (Hint.Unlikely(bucketB.xmins[(int)(j + 15)] > currentX)) - break; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 1]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 2]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 3]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 4]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 5]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 6]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 7]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 8]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 9]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 10]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 11]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 12]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 13]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 14]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j + 15]) == 0) - nextHitIndex++; - pair++; - j += 16; - - //tests += 16; - - if (Hint.Unlikely(nextHitIndex >= 1008)) - { - //drain0Marker.Begin(); - drain.Drain(nextHitIndex); - //drain0Marker.End(); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - - //if (nextHitIndex > 10) - //{ - // UnityEngine.Debug.Log($"i : {i}, j : {j}, drain: {nextHitIndex}"); - //} - } - //inner0Marker.End(); - - //cleanup0Marker.Begin(); - while (pair < final && bucketB.xmins[(int)j] <= currentX) - { - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrB[j]) == 0) - nextHitIndex++; - pair++; - j++; - //tests++; - } - //cleanup0Marker.End(); - - if (nextHitIndex >= 1008) - { - //drain0Marker.Begin(); - drain.Drain(nextHitIndex); - //drain0Marker.End(); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - outer0Marker.End(); - outer1Marker.Begin(); - - //Check for a starting in b's x range - int astart = 0; - for (int i = 0; i < countB; i++) - { - //Advance to a.xmin > b.xmin - //Exclude equals case this time by continuing if equal - //lead1Marker.Begin(); - while (astart < countA && bucketA.xmins[astart] <= bucketB.xmins[i]) - astart++; - //lead1Marker.End(); - if (astart >= countA) - break; - - float4 current = -minMaxPtrB[i].zwxy; - - float currentX = bucketB.xmaxs[i]; - - ulong j = (ulong)astart; - - ulong bucketsSum = (ulong)bucketA.count + (ulong)bucketB.count; - - ulong pair = ((((ulong)i) + bucketsSum) << 32) | j; - ulong final = ((((ulong)i) + bucketsSum) << 32) | ((uint)countA); - - //inner1Marker.Begin(); - while (pair + 15 < final) - { - if (Hint.Unlikely(bucketA.xmins[(int)(j + 15)] > currentX)) - break; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 1]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 2]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 3]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 4]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 5]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 6]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 7]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 8]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 9]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 10]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 11]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 12]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 13]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 14]) == 0) - nextHitIndex++; - pair++; - - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j + 15]) == 0) - nextHitIndex++; - pair++; - j += 16; - //tests += 16; - - if (Hint.Unlikely(nextHitIndex >= 1008)) - { - //drain1Marker.Begin(); - drain.Drain(nextHitIndex); - //drain1Marker.End(); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - //inner1Marker.End(); - - //cleanup1Marker.Begin(); - while (pair < final && bucketA.xmins[(int)j] <= currentX) - { - hitCachePtr[nextHitIndex] = pair; - if (math.bitmask(current < minMaxPtrA[j]) == 0) - nextHitIndex++; - pair++; - j++; - //tests++; - } - //cleanup1Marker.End(); - - if (nextHitIndex >= 1008) - { - //drain1Marker.Begin(); - drain.Drain(nextHitIndex); - //drain1Marker.End(); - hitCachePtr = drain.AcquireDrainBuffer1024ForWrite(); - nextHitIndex = 0; - } - } - - outer1Marker.End(); - - drain0Marker.Begin(); - drain.Drain(nextHitIndex); - drain0Marker.End(); - - //if (tests > 10000) - // UnityEngine.Debug.Log($"Unrolled tests: {tests}"); - } - #endregion - - #region Broken - // Todo: Fix for sign flip - static void SelfSweepDualGenAndStats(BucketSlices bucket, in FixedString128Bytes layerName) - { - if (bucket.count <= 1) - return; - - var zToXMinsMaxes = new NativeArray(2 * bucket.count, Allocator.Temp); - var xs = new NativeArray(2 * bucket.count, Allocator.Temp); - - var xSort = new NativeArray(bucket.count * 2, Allocator.Temp); - var zSort = new NativeArray(bucket.count * 2, Allocator.Temp); - - for (int i = 0; i < bucket.count; i++) - { - var xmin = bucket.xmins[i]; - var xmax = bucket.xmaxs[i]; - var minYZmaxYZ = bucket.yzminmaxs[i]; - xSort[2 * i] = new Sortable { f = xmin, index = (uint)i }; - xSort[2 * i + 1] = new Sortable { f = xmax, index = (uint)i + (uint)bucket.count }; - - zSort[2 * i] = new Sortable { f = minYZmaxYZ.y, index = (uint)i }; - zSort[2 * i + 1] = new Sortable { f = minYZmaxYZ.w, index = (uint)i + (uint)bucket.count }; - } - - xSort.Sort(); - zSort.Sort(); - - for (int i = 0; i < xSort.Length; i++) - { - xs[i] = xSort[i].index; - zToXMinsMaxes[i] = zSort[i].index; - } - - var minYZmaxYZs = bucket.yzminmaxs; - - var zIntervals = new NativeList(minYZmaxYZs.Length, Allocator.Temp); - zIntervals.ResizeUninitialized(minYZmaxYZs.Length); - - var zBits = new NativeList(minYZmaxYZs.Length / 64 + 1, Allocator.Temp); - zBits.Resize(minYZmaxYZs.Length / 64 + 1, NativeArrayOptions.ClearMemory); - - { - int minBit = 0; - int index = 0; - for (int i = 0; i < zToXMinsMaxes.Length; i++) - { - if (zToXMinsMaxes[i] < minYZmaxYZs.Length) - { - ref var interval = ref zIntervals.ElementAt((int)zToXMinsMaxes[i]); - interval.index = index; - interval.min = minBit; - ref var bitField = ref zBits.ElementAt(index >> 6); - bitField.SetBits(index & 0x3f, true); - index++; - } - else - { - ref var interval = ref zIntervals.ElementAt((int)(zToXMinsMaxes[i] - (uint)minYZmaxYZs.Length)); - interval.max = index; - ref var bitField = ref zBits.ElementAt(interval.index >> 6); - bitField.SetBits(interval.index & 0x3f, false); - if (interval.index == minBit) - { - while (minBit <= index) - { - var scanBits = zBits.ElementAt(minBit >> 6); - var tzcnt = scanBits.CountTrailingZeros(); - if (tzcnt < 64) - { - minBit = (minBit & ~0x3f) + tzcnt; - break; - } - minBit = (minBit & ~0x3f) + 64; - } - minBit = math.min(minBit, index + 1); - } - } - } - } - - var zToXs = new NativeArray(minYZmaxYZs.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); - - int hitCount = 0; - int innerLoopEnterCount = 0; - int innerLoopTestCount = 0; - int innerLoopRunMin = int.MaxValue; - int innerLoopRunMax = 0; - int maxRunIntervalIndex = 0; - int touchedZeroBitfield = 0; - - for (int i = 0; i < xs.Length; i++) - { - if (xs[i] < minYZmaxYZs.Length) - { - int runCount = 0; - - var interval = zIntervals[(int)xs[i]]; - int minBitfield = interval.min >> 6; - int maxBitfield = interval.max >> 6; - if (minBitfield == maxBitfield) - { - int minBit = interval.min & 0x3f; - int maxBit = interval.max & 0x3f; - var bitField = zBits[minBitfield]; - if (minBit > 0) - bitField.SetBits(0, false, minBit); - - if (bitField.Value == 0) - touchedZeroBitfield++; - - for (var j = bitField.CountTrailingZeros(); j <= maxBit; bitField.SetBits(j, false), j = bitField.CountTrailingZeros()) - { - runCount++; - var currentIndex = (int)xs[i]; - var otherIndex = zToXs[j + 64 * minBitfield]; - - float4 less = math.shuffle(minYZmaxYZs[currentIndex], - minYZmaxYZs[otherIndex], - math.ShuffleComponent.RightZ, - math.ShuffleComponent.RightW, - math.ShuffleComponent.LeftZ, - math.ShuffleComponent.LeftW - ); - float4 more = math.shuffle(minYZmaxYZs[currentIndex], - minYZmaxYZs[otherIndex], - math.ShuffleComponent.LeftX, - math.ShuffleComponent.LeftY, - math.ShuffleComponent.RightX, - math.ShuffleComponent.RightY - ); - - if (math.bitmask(less < more) == 0) - { - //overlaps.Add(new EntityPair(entities[currentIndex], entities[otherIndex])); - hitCount++; - } - } - } - else - { - { - int minBit = interval.min & 0x3f; - var bitField = zBits[minBitfield]; - if (minBit > 0) - bitField.SetBits(0, false, minBit); - - if (bitField.Value == 0) - touchedZeroBitfield++; - - for (var j = bitField.CountTrailingZeros(); j < 64; bitField.SetBits(j, false), j = bitField.CountTrailingZeros()) - { - runCount++; - var currentIndex = (int)xs[i]; - var otherIndex = zToXs[j + 64 * minBitfield]; - - float4 less = math.shuffle(minYZmaxYZs[currentIndex], - minYZmaxYZs[otherIndex], - math.ShuffleComponent.RightZ, - math.ShuffleComponent.RightW, - math.ShuffleComponent.LeftZ, - math.ShuffleComponent.LeftW - ); - float4 more = math.shuffle(minYZmaxYZs[currentIndex], - minYZmaxYZs[otherIndex], - math.ShuffleComponent.LeftX, - math.ShuffleComponent.LeftY, - math.ShuffleComponent.RightX, - math.ShuffleComponent.RightY - ); - - if (math.bitmask(less < more) == 0) - { - //overlaps.Add(new EntityPair(entities[currentIndex], entities[otherIndex])); - hitCount++; - } - } - } - - for (int k = minBitfield + 1; k < maxBitfield; k++) - { - var bitField = zBits[k]; - - if (bitField.Value == 0) - touchedZeroBitfield++; - - for (var j = bitField.CountTrailingZeros(); j < 64; bitField.SetBits(j, false), j = bitField.CountTrailingZeros()) - { - runCount++; - var currentIndex = (int)xs[i]; - var otherIndex = zToXs[j + 64 * k]; - - float4 less = math.shuffle(minYZmaxYZs[currentIndex], - minYZmaxYZs[otherIndex], - math.ShuffleComponent.RightZ, - math.ShuffleComponent.RightW, - math.ShuffleComponent.LeftZ, - math.ShuffleComponent.LeftW - ); - float4 more = math.shuffle(minYZmaxYZs[currentIndex], - minYZmaxYZs[otherIndex], - math.ShuffleComponent.LeftX, - math.ShuffleComponent.LeftY, - math.ShuffleComponent.RightX, - math.ShuffleComponent.RightY - ); - - if (math.bitmask(less < more) == 0) - { - //overlaps.Add(new EntityPair(entities[currentIndex], entities[otherIndex])); - hitCount++; - } - } - } - - { - int maxBit = interval.max & 0x3f; - var bitField = zBits[maxBitfield]; - - if (bitField.Value == 0) - touchedZeroBitfield++; - - for (var j = bitField.CountTrailingZeros(); j <= maxBit; bitField.SetBits(j, false), j = bitField.CountTrailingZeros()) - { - runCount++; - var currentIndex = (int)xs[i]; - var otherIndex = zToXs[j + 64 * maxBitfield]; - - float4 less = math.shuffle(minYZmaxYZs[currentIndex], - minYZmaxYZs[otherIndex], - math.ShuffleComponent.RightZ, - math.ShuffleComponent.RightW, - math.ShuffleComponent.LeftZ, - math.ShuffleComponent.LeftW - ); - float4 more = math.shuffle(minYZmaxYZs[currentIndex], - minYZmaxYZs[otherIndex], - math.ShuffleComponent.LeftX, - math.ShuffleComponent.LeftY, - math.ShuffleComponent.RightX, - math.ShuffleComponent.RightY - ); - - if (math.bitmask(less < more) == 0) - { - //overlaps.Add(new EntityPair(entities[currentIndex], entities[otherIndex])); - hitCount++; - } - } - } - } - - ref var currentBitfield = ref zBits.ElementAt(interval.index >> 6); - currentBitfield.SetBits(interval.index & 0x3f, true); - zToXs[interval.index] = (int)xs[i]; - - if (runCount > 0) - innerLoopEnterCount++; - innerLoopTestCount += runCount; - if (runCount > innerLoopRunMax) - maxRunIntervalIndex = (int)xs[i]; - innerLoopRunMax = math.max(innerLoopRunMax, runCount); - innerLoopRunMin = math.min(innerLoopRunMin, runCount); - } - else - { - var interval = zIntervals[(int)(xs[i] - minYZmaxYZs.Length)]; - ref var currentBitfield = ref zBits.ElementAt(interval.index >> 6); - currentBitfield.SetBits(interval.index & 0x3f, false); - } - } - var maxInterval = zIntervals[maxRunIntervalIndex]; - - UnityEngine.Debug.Log( - $"Dual Self Sweep stats for layer {layerName} at bucket index {bucket.bucketIndex} and count {bucket.count}\nHits: {hitCount}, inner loop enters: {innerLoopEnterCount}, inner loop tests: {innerLoopTestCount}, inner loop run (min, max): ({innerLoopRunMin}, {innerLoopRunMax}), maxInterval: ({maxInterval.min}, {maxInterval.index}, {maxInterval.max}), touched zero bitfields: {touchedZeroBitfield}"); - } - - struct ZInterval - { - public int index; - public int min; - public int max; - } - - struct Sortable : System.IComparable - { - public float f; - public uint index; - - public int CompareTo(Sortable other) - { - var result = f.CompareTo(other.f); - if (result == 0) - return index.CompareTo(other.index); - return result; } } #endregion diff --git a/PsyshockPhysics/Physics/Internal/Queries/Layers/ForEachPairInternal.cs b/PsyshockPhysics/Physics/Internal/Queries/Layers/ForEachPairInternal.cs new file mode 100644 index 0000000..8b04219 --- /dev/null +++ b/PsyshockPhysics/Physics/Internal/Queries/Layers/ForEachPairInternal.cs @@ -0,0 +1,486 @@ +using System.Collections.Generic; +using System.Diagnostics; +using Unity.Burst; +using Unity.Collections; +using Unity.Entities; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Latios.Psyshock +{ + public partial struct ForEachPairConfig + { + internal enum ScheduleMode + { + Single, + ParallelPart1, + ParallelPart2, + ParallelUnsafe + } + + internal static class ForEachPairInternal + { + [BurstCompile] + public struct ForEachPairJob : IJobFor + { + [NativeDisableParallelForRestriction] public PairStream pairStream; + T processor; + Unity.Profiling.ProfilerMarker modeAndTMarker; + ScheduleMode mode; + bool includeDisabled; + + public ForEachPairJob(in PairStream pairStream, in T processor, bool includeDisabled) + { + this.pairStream = pairStream; + this.processor = processor; + this.includeDisabled = includeDisabled; + modeAndTMarker = default; + mode = default; + } + + public void Run() + { + SetScheduleMode(ScheduleMode.Single); + this.Run(1); + } + + public JobHandle ScheduleSingle(JobHandle inputDeps) + { + SetScheduleMode(ScheduleMode.Single); + return this.Schedule(1, inputDeps); + } + + public JobHandle ScheduleParallel(JobHandle inputDeps, ScheduleMode scheduleMode) + { + SetScheduleMode(scheduleMode); + if (scheduleMode == ScheduleMode.ParallelPart1) +#if ENABLE_UNITY_COLLECTIONS_CHECKS + return this.ScheduleParallel(pairStream.data.expectedBucketCount + 2, 1, inputDeps); +#else + return this.ScheduleParallel(pairStream.data.expectedBucketCount + 1, 1, inputDeps); +#endif + if (scheduleMode == ScheduleMode.ParallelPart2) + return this.ScheduleParallel(2 * pairStream.data.expectedBucketCount - 2, 1, inputDeps); + if (scheduleMode == ScheduleMode.ParallelUnsafe) + return this.ScheduleParallel(pairStream.mixedIslandAggregateStream, 1, inputDeps); + return inputDeps; + } + + void SetScheduleMode(ScheduleMode scheduleMode) + { + mode = scheduleMode; + FixedString64Bytes modeString = default; + if (scheduleMode == ScheduleMode.Single) + modeString = "Single"; + else if (scheduleMode == ScheduleMode.ParallelPart1) + modeString = "ParallelPart1"; + else if (scheduleMode == ScheduleMode.ParallelPart2) + modeString = "ParallelPart2"; + else if (scheduleMode == ScheduleMode.ParallelUnsafe) + modeString = "ParallelUnsafe"; + + if (includeDisabled) + modeString = $"{modeString}_includeDisabled"; + + bool isBurst = true; + IsBurst(ref isBurst); + if (isBurst) + { + modeAndTMarker = new Unity.Profiling.ProfilerMarker($"{modeString}_{processor}"); + } + else + { + FixedString128Bytes processorName = default; + GetProcessorNameNoBurst(ref processorName); + modeAndTMarker = new Unity.Profiling.ProfilerMarker($"{modeString}_{processorName}"); + } + } + + [BurstDiscard] + static void IsBurst(ref bool isBurst) => isBurst = false; + + [BurstDiscard] + static void GetProcessorNameNoBurst(ref FixedString128Bytes name) + { + name = nameof(T); + } + + public unsafe void Execute(int jobIndex) + { + using var jobName = modeAndTMarker.Auto(); + if (mode == ScheduleMode.Single) + { + ForEachPairMethods.ExecuteBatch(ref pairStream, ref processor, 0, pairStream.mixedIslandAggregateStream, true, true, includeDisabled); + } + else if (mode == ScheduleMode.ParallelPart1) + { + if (jobIndex < pairStream.data.expectedBucketCount) + { + ForEachPairMethods.ExecuteBatch(ref pairStream, ref processor, jobIndex * 3, 3, true, true, includeDisabled); + } + else if (jobIndex == pairStream.data.expectedBucketCount) + { + ForEachPairMethods.ExecuteBatch(ref pairStream, ref processor, pairStream.nanBucketStream, 1, true, true, includeDisabled); + } + else if (jobIndex == pairStream.data.expectedBucketCount + 1) + { + if (pairStream.data.state->needsIslanding) + ForEachPairMethods.CreateIslands(ref pairStream); + } + else if (jobIndex == pairStream.data.expectedBucketCount + 2) + { + if (pairStream.data.state->needsAliasChecks) + ForEachPairMethods.CheckAliasing(pairStream); + } + } + else if (mode == ScheduleMode.ParallelPart2) + { + ForEachPairMethods.ExecuteBatch(ref pairStream, ref processor, jobIndex + pairStream.firstMixedBucketStream, 1, true, true, includeDisabled); + } + else if (mode == ScheduleMode.ParallelUnsafe) + { + ForEachPairMethods.ExecuteBatch(ref pairStream, ref processor, jobIndex, 1, false, true, includeDisabled); + } + } + + [UnityEngine.Scripting.Preserve] + void RequireEarlyJobInit() + { + new InitJobsForProcessors.ForeachIniter().Init(); + } + } + } + } + + internal static unsafe class ForEachPairMethods + { + public static void ExecuteBatch(ref PairStream pairStream, ref T processor, int startIndex, int count, bool isThreadSafe, bool isKeySafe, + bool includeDisabled) where T : struct, + IForEachPairProcessor + { + var enumerator = pairStream.GetEnumerator(); + enumerator.enumerator = pairStream.data.pairHeaders.GetEnumerator(startIndex); + enumerator.pair.index = startIndex; + enumerator.pair.areEntitiesSafeInContext = isThreadSafe; + enumerator.pair.isParallelKeySafe = isKeySafe; + enumerator.onePastLastStreamIndex = startIndex + count; + + var context = new ForEachPairBatchContext { enumerator = enumerator }; + if (processor.BeginStreamBatch(context)) + { + while (enumerator.MoveNext()) + { + if (includeDisabled || enumerator.pair.enabled) + processor.Execute(ref enumerator.pair); + } + processor.EndStreamBatch(context); + } + } + + public static void CreateIslands(ref PairStream pairStream) + { + pairStream.CheckWriteAccess(); + + var aggregateStream = pairStream.mixedIslandAggregateStream; + pairStream.data.pairHeaders.ClearIndex(aggregateStream); + for (int i = pairStream.firstMixedBucketStream; i < pairStream.nanBucketStream; i++) + { + pairStream.data.pairHeaders.MoveIndexToOtherIndexUnordered(i, aggregateStream); + } + + int totalPairs = pairStream.data.pairHeaders.CountForIndex(aggregateStream); + var entityLookup = new NativeHashMap(totalPairs, Allocator.Temp); + var ranks = new NativeList(totalPairs, Allocator.Temp); + var stack = new NativeList(32, Allocator.Temp); + + // Join islands + var enumerator = pairStream.data.pairHeaders.GetEnumerator(aggregateStream); + while (enumerator.MoveNext()) + { + ref var header = ref enumerator.GetCurrentAsRef(); + + bool addedA = entityLookup.TryGetValue(header.entityA, out var indexA); + bool addedB = entityLookup.TryGetValue(header.entityB, out var indexB); + + if (!addedA && !addedB) + { + var index = ranks.Length; + ranks.AddNoResize(index); + entityLookup.Add(header.entityA, index); + entityLookup.Add(header.entityB, index); + } + else if (addedA && !addedB) + { + entityLookup.Add(header.entityB, indexA); + } + else if (!addedA && addedB) + { + entityLookup.Add(header.entityA, indexB); + } + else + { + bool aDirty = false; + bool bDirty = false; + if (ranks[indexA] != indexA) + { + indexA = CollapseRanks(indexA, ref ranks, ref stack); + aDirty = true; + } + if (ranks[indexB] != indexB) + { + indexB = CollapseRanks(indexB, ref ranks, ref stack); + bDirty = true; + } + if (indexA < indexB) + { + ranks[indexB] = indexA; + indexB = indexA; + bDirty = true; + } + else if (indexB < indexA) + { + ranks[indexA] = indexB; + indexA = indexB; + aDirty = true; + } + if (aDirty) + entityLookup[header.entityA] = indexA; + if (bDirty) + entityLookup[header.entityB] = indexB; + } + } + + // Collapse any ranks not yet collapsed and identify unique islands + var uniqueIslandIndices = new NativeList(ranks.Length, Allocator.Temp); + for (int i = 0; i < ranks.Length; i++) + { + var parent = ranks[i]; + if (parent != i) + ranks[i] = ranks[parent]; + else + uniqueIslandIndices.Add(i); + } + + // Remap ranks to unique island indices + for (int islandIndex = 0, i = 0; i < ranks.Length; i++) + { + if (i == uniqueIslandIndices[islandIndex]) + ranks[i] = islandIndex; + else + ranks[i] = ranks[ranks[i]]; + } + + // If we have more islands than streams, we need to distribute them. + if (uniqueIslandIndices.Length > pairStream.nanBucketStream - pairStream.firstMixedBucketStream) + { + // Count elements in each island and sort + var islandIndicesAndCounts = new NativeArray(uniqueIslandIndices.Length, Allocator.Temp); + for (int i = 0; i < uniqueIslandIndices.Length; i++) + islandIndicesAndCounts[i] = new int2(i, 0); + enumerator = pairStream.data.pairHeaders.GetEnumerator(aggregateStream); + while (enumerator.MoveNext()) + { + var entityA = enumerator.GetCurrentAsRef().entityA; + var targetIsland = ranks[entityLookup[entityA]]; + islandIndicesAndCounts[targetIsland] += new int2(0, 1); + } + islandIndicesAndCounts.Sort(new GreatestToLeastComparer()); + + // Distribute largest islands + var streamIndicesAndCounts = new NativeArray(pairStream.nanBucketStream - pairStream.firstMixedBucketStream, Allocator.Temp); + for (int i = 0; i < streamIndicesAndCounts.Length; i++) + { + var island = islandIndicesAndCounts[i]; + streamIndicesAndCounts[i] = new int2(i, island.y); + uniqueIslandIndices[island.x] = i; + } + + // Distribute remaining islands using a greedy algorithm that distributes each island to the stream + // with the least elements. + int currentStream = streamIndicesAndCounts.Length - 1; + int lowestCountInAPreviousStream = int.MaxValue; + int countInNextStream = streamIndicesAndCounts[streamIndicesAndCounts.Length - 2].y; + for (int i = streamIndicesAndCounts.Length; i < islandIndicesAndCounts.Length; i++) + { + var island = islandIndicesAndCounts[i]; + streamIndicesAndCounts[currentStream] += new int2(0, island.y); + var stream = streamIndicesAndCounts[currentStream]; + uniqueIslandIndices[island.x] = stream.x; + + if (stream.y < math.max(lowestCountInAPreviousStream, countInNextStream)) + continue; + + if (countInNextStream <= lowestCountInAPreviousStream) + { + lowestCountInAPreviousStream = math.min(lowestCountInAPreviousStream, stream.y); + currentStream--; + if (currentStream == 0) + countInNextStream = int.MaxValue; + else + countInNextStream = streamIndicesAndCounts[currentStream - 1].y; + continue; + } + + // At this point, a previous stream has the minimum. Re-sort the streams and start from the back again. + streamIndicesAndCounts.Sort(new GreatestToLeastComparer()); + currentStream = streamIndicesAndCounts.Length - 1; + lowestCountInAPreviousStream = int.MaxValue; + countInNextStream = streamIndicesAndCounts[currentStream - 1].y; + + if (stream.y == 1) + { + // Use a single-element distribution loop in case we end up with lots of independent islands + while (i < islandIndicesAndCounts.Length) + { + for (int currentCount = streamIndicesAndCounts[currentStream].y; i < islandIndicesAndCounts.Length && currentCount < countInNextStream; currentCount++) + { + for (int streamIndex = currentStream; i < islandIndicesAndCounts.Length && streamIndex < streamIndicesAndCounts.Length; streamIndex++, i++) + { + island = islandIndicesAndCounts[i]; + streamIndicesAndCounts[streamIndex] += new int2(0, island.y); + stream = streamIndicesAndCounts[streamIndex]; + uniqueIslandIndices[island.x] = stream.x; + } + } + currentStream--; + if (currentStream == 0) + countInNextStream = int.MaxValue; + else + countInNextStream = streamIndicesAndCounts[currentStream - 1].y; + } + } + } + + // At this point, uniqueIslandIndices contains stream indices. Assign to ranks. + for (int i = 0; i < ranks.Length; i++) + { + ranks[i] = uniqueIslandIndices[ranks[i]]; + } + } + + // At this point, ranks contain stream indices offset from the first mixed bucket stream. + // We can now redistribute the pair headers. + var baseStream = pairStream.firstMixedBucketStream; + enumerator = pairStream.data.pairHeaders.GetEnumerator(aggregateStream); + while (enumerator.MoveNext()) + { + ref var header = ref enumerator.GetCurrentAsRef(); + int targetStream = ranks[entityLookup[header.entityA]]; + pairStream.data.pairHeaders.Write(header, baseStream + targetStream); + } + + pairStream.data.state->needsIslanding = false; + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + public static void CheckAliasing(PairStream pairStream) + { + // Todo: If we ever make a readonly mode, this should be checking read access and not update the state flag at the end. + pairStream.CheckWriteAccess(); + + int pairsToCheck = 0; + for (int i = 0; i < pairStream.firstMixedBucketStream; i++) + { + pairsToCheck += pairStream.data.pairHeaders.CountForIndex(i); + } + + var map = new NativeHashMap(pairsToCheck, Allocator.Temp); + + for (int i = 0; i < pairStream.firstMixedBucketStream; i++) + { + var bucketIndex = i / 3; + var enumerator = pairStream.data.pairHeaders.GetEnumerator(i); + while (enumerator.MoveNext()) + { + ref var header = ref enumerator.GetCurrentAsRef(); + bool aIsRW = (header.flags & PairStream.PairHeader.kWritableA) == PairStream.PairHeader.kWritableA; + if (aIsRW && map.TryGetValue(header.entityA, out var oldIndex)) + { + if (oldIndex != i) + throw new System.InvalidOperationException( + $"Entity {header.entityA.ToFixedString()} is contained in both buckets {oldIndex} and {bucketIndex} within the PairStream. This is not allowed."); + } + else if (aIsRW) + map.Add(header.entityA, bucketIndex); + + bool bIsRW = (header.flags & PairStream.PairHeader.kWritableB) == PairStream.PairHeader.kWritableB; + if (bIsRW && map.TryGetValue(header.entityB, out oldIndex)) + { + if (oldIndex != i) + throw new System.InvalidOperationException( + $"Entity {header.entityB.ToFixedString()} is contained in both buckets {oldIndex} and {bucketIndex} within the PairStream. This is not allowed."); + } + else if (bIsRW) + map.Add(header.entityB, bucketIndex); + } + } + + { + var enumerator = pairStream.data.pairHeaders.GetEnumerator(pairStream.nanBucketStream); + while (enumerator.MoveNext()) + { + ref var header = ref enumerator.GetCurrentAsRef(); + bool aIsRW = (header.flags & PairStream.PairHeader.kWritableA) == PairStream.PairHeader.kWritableA; + if (aIsRW && map.TryGetValue(header.entityA, out var oldIndex)) + { + throw new System.InvalidOperationException( + $"Entity {header.entityA.ToFixedString()} is contained in both buckets {oldIndex} and the NaN bucket within the PairStream. This is not allowed."); + } + + bool bIsRW = (header.flags & PairStream.PairHeader.kWritableB) == PairStream.PairHeader.kWritableB; + if (bIsRW && map.TryGetValue(header.entityB, out oldIndex)) + { + throw new System.InvalidOperationException( + $"Entity {header.entityB.ToFixedString()} is contained in both buckets {oldIndex} and the NaN bucket within the PairStream. This is not allowed."); + } + } + } + + pairStream.data.state->needsAliasChecks = false; + } + + public static void ScheduleBumpVersions(ref PairStream pairStream, ref JobHandle jobHandle) + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + jobHandle = new BumpVersionsJob { pairStream = pairStream }.Schedule(jobHandle); +#endif + } + + static int CollapseRanks(int tail, ref NativeList ranks, ref NativeList stack) + { + stack.Clear(); + stack.Add(tail); + int current = ranks[tail]; + while (ranks[current] != current) + { + stack.Add(current); + current = ranks[current]; + } + for (int i = 0; i < stack.Length; i++) + ranks[stack[i]] = current; + return current; + } + + struct GreatestToLeastComparer : IComparer + { + public int Compare(int2 x, int2 y) + { + return -x.y.CompareTo(y.y); + } + } + + [BurstCompile] + struct BumpVersionsJob : IJob + { + public PairStream pairStream; + + public void Execute() + { + pairStream.CheckWriteAccess(); + pairStream.data.state->enumeratorVersion++; + pairStream.data.state->pairPtrVersion++; + } + } + } +} + diff --git a/PsyshockPhysics/Physics/Internal/Queries/Layers/ForEachPairInternal.cs.meta b/PsyshockPhysics/Physics/Internal/Queries/Layers/ForEachPairInternal.cs.meta new file mode 100644 index 0000000..016fa4c --- /dev/null +++ b/PsyshockPhysics/Physics/Internal/Queries/Layers/ForEachPairInternal.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ac822a67688bc6f4ebe0827e08400be3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/PsyshockPhysics/Physics/Internal/Queries/PointRayCollider/PointRayBox.cs b/PsyshockPhysics/Physics/Internal/Queries/PointRayCollider/PointRayBox.cs index 3b2c2cd..b8caf18 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/PointRayCollider/PointRayBox.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/PointRayCollider/PointRayBox.cs @@ -299,27 +299,27 @@ internal static void BestFacePlanesAndVertices(in BoxCollider box, switch (faceIndex) { case 0: // positive X - plane = new Plane(new float3(1f, 0f, 0f), box.halfSize.x); + plane = new Plane(new float3(1f, 0f, 0f), -box.halfSize.x); vertices = new simdFloat3(ones, firstComponent, secondCompPos); break; case 1: // positive Y - plane = new Plane(new float3(0f, 1f, 0f), box.halfSize.y); + plane = new Plane(new float3(0f, 1f, 0f), -box.halfSize.y); vertices = new simdFloat3(firstComponent, ones, secondCompPos); break; case 2: // positive Z - plane = new Plane(new float3(0f, 0f, 1f), box.halfSize.z); + plane = new Plane(new float3(0f, 0f, 1f), -box.halfSize.z); vertices = new simdFloat3(firstComponent, secondCompPos, ones); break; case 3: // negative X - plane = new Plane(new float3(-1f, 0f, 0f), -box.halfSize.x); + plane = new Plane(new float3(-1f, 0f, 0f), box.halfSize.x); vertices = new simdFloat3(-ones, firstComponent, -secondCompPos); break; case 4: // negative Y - plane = new Plane(new float3(0f, -1f, 0f), -box.halfSize.y); + plane = new Plane(new float3(0f, -1f, 0f), box.halfSize.y); vertices = new simdFloat3(firstComponent, -ones, -secondCompPos); break; case 5: // negative Z - plane = new Plane(new float3(0f, 0f, -1f), -box.halfSize.z); + plane = new Plane(new float3(0f, 0f, -1f), box.halfSize.z); vertices = new simdFloat3(firstComponent, -secondCompPos, -ones); break; default: // Should not happen diff --git a/PsyshockPhysics/Physics/Internal/Queries/PointRayCollider/PointRayDispatch.cs b/PsyshockPhysics/Physics/Internal/Queries/PointRayCollider/PointRayDispatch.cs index 697b4e9..c5f8e71 100644 --- a/PsyshockPhysics/Physics/Internal/Queries/PointRayCollider/PointRayDispatch.cs +++ b/PsyshockPhysics/Physics/Internal/Queries/PointRayCollider/PointRayDispatch.cs @@ -32,11 +32,11 @@ public static bool DistanceBetween(float3 point, in Collider collider, in Transf Physics.ScaleStretchCollider(ref convex, transform.scale, transform.stretch); return PointRayConvex.DistanceBetween(point, in convex, in rigidTransform, maxDistance, out result); case ColliderType.TriMesh: - var triMesh = collider.m_triMesh; + var triMesh = collider.m_triMesh(); Physics.ScaleStretchCollider(ref triMesh, transform.scale, transform.stretch); return PointRayTriMesh.DistanceBetween(point, in triMesh, in rigidTransform, maxDistance, out result); case ColliderType.Compound: - var compound = collider.m_compound; + var compound = collider.m_compound(); Physics.ScaleStretchCollider(ref compound, transform.scale, transform.stretch); return PointRayCompound.DistanceBetween(point, in compound, in rigidTransform, maxDistance, out result); default: @@ -71,11 +71,11 @@ public static bool Raycast(in Ray ray, in Collider collider, in TransformQvvs tr Physics.ScaleStretchCollider(ref convex, transform.scale, transform.stretch); return PointRayConvex.Raycast(in ray, in convex, in rigidTransform, out result); case ColliderType.TriMesh: - var triMesh = collider.m_triMesh; + var triMesh = collider.m_triMesh(); Physics.ScaleStretchCollider(ref triMesh, transform.scale, transform.stretch); return PointRayTriMesh.Raycast(in ray, in triMesh, in rigidTransform, out result); case ColliderType.Compound: - var compound = collider.m_compound; + var compound = collider.m_compound(); Physics.ScaleStretchCollider(ref compound, transform.scale, transform.stretch); return PointRayCompound.Raycast(in ray, in compound, in rigidTransform, out result); default: diff --git a/PsyshockPhysics/Physics/Spatial/Builders/Physics.BuildCollisionLayer.cs b/PsyshockPhysics/Physics/Spatial/Builders/Physics.BuildCollisionLayer.cs index 15aea19..1c00f63 100644 --- a/PsyshockPhysics/Physics/Spatial/Builders/Physics.BuildCollisionLayer.cs +++ b/PsyshockPhysics/Physics/Spatial/Builders/Physics.BuildCollisionLayer.cs @@ -3,6 +3,7 @@ using Latios.Transforms.Abstract; using Unity.Collections; using Unity.Entities; +using Unity.Entities.Exposed; using Unity.Jobs; using Unity.Mathematics; @@ -68,16 +69,17 @@ public struct BuildCollisionLayerConfig internal BuildCollisionLayerTypeHandles typeGroup; internal EntityQuery query; - internal NativeArray aabbs; - internal NativeArray bodies; + internal NativeArray aabbsArray; + internal NativeArray bodiesArray; + internal NativeList aabbsList; + internal NativeList bodiesList; internal CollisionLayerSettings settings; internal bool hasQueryData; internal bool hasBodiesArray; internal bool hasAabbsArray; - - internal int count; + internal bool usesLists; /// /// The default CollisionLayerSettings used when none is specified. @@ -109,12 +111,13 @@ public static FluentQuery PatchQueryForBuildingCollisionLayer(this FluentQuery f /// The system used for extracting ComponentTypeHandles public static BuildCollisionLayerConfig BuildCollisionLayer(EntityQuery query, SystemBase system) { - var config = new BuildCollisionLayerConfig(); - config.query = query; - config.typeGroup = new BuildCollisionLayerTypeHandles(system); - config.hasQueryData = true; - config.settings = BuildCollisionLayerConfig.defaultSettings; - config.count = query.CalculateEntityCount(); + var config = new BuildCollisionLayerConfig + { + query = query, + typeGroup = new BuildCollisionLayerTypeHandles(system), + hasQueryData = true, + settings = BuildCollisionLayerConfig.defaultSettings, + }; return config; } @@ -126,12 +129,13 @@ public static BuildCollisionLayerConfig BuildCollisionLayer(EntityQuery query, S /// The system used for extracting ComponentTypeHandles public static BuildCollisionLayerConfig BuildCollisionLayer(EntityQuery query, ref SystemState system) { - var config = new BuildCollisionLayerConfig(); - config.query = query; - config.typeGroup = new BuildCollisionLayerTypeHandles(ref system); - config.hasQueryData = true; - config.settings = BuildCollisionLayerConfig.defaultSettings; - config.count = query.CalculateEntityCount(); + var config = new BuildCollisionLayerConfig + { + query = query, + typeGroup = new BuildCollisionLayerTypeHandles(ref system), + hasQueryData = true, + settings = BuildCollisionLayerConfig.defaultSettings, + }; return config; } @@ -143,12 +147,13 @@ public static BuildCollisionLayerConfig BuildCollisionLayer(EntityQuery query, r /// Cached type handles that must be updated before invoking this method public static BuildCollisionLayerConfig BuildCollisionLayer(EntityQuery query, in BuildCollisionLayerTypeHandles requiredTypeHandles) { - var config = new BuildCollisionLayerConfig(); - config.query = query; - config.typeGroup = requiredTypeHandles; - config.hasQueryData = true; - config.settings = BuildCollisionLayerConfig.defaultSettings; - config.count = query.CalculateEntityCount(); + var config = new BuildCollisionLayerConfig + { + query = query, + typeGroup = requiredTypeHandles, + hasQueryData = true, + settings = BuildCollisionLayerConfig.defaultSettings, + }; return config; } @@ -158,11 +163,12 @@ public static BuildCollisionLayerConfig BuildCollisionLayer(EntityQuery query, i /// The array of ColliderBody instances which should be baked into the CollisionLayer public static BuildCollisionLayerConfig BuildCollisionLayer(NativeArray bodies) { - var config = new BuildCollisionLayerConfig(); - config.bodies = bodies; - config.hasBodiesArray = true; - config.settings = BuildCollisionLayerConfig.defaultSettings; - config.count = bodies.Length; + var config = new BuildCollisionLayerConfig + { + bodiesArray = bodies, + hasBodiesArray = true, + settings = BuildCollisionLayerConfig.defaultSettings, + }; return config; } @@ -171,18 +177,57 @@ public static BuildCollisionLayerConfig BuildCollisionLayer(NativeArray /// The array of ColliderBody instances which should be baked into the CollisionLayer - /// The array of AABBs parallel to the bodies array specifying different AABBs to use for each body + /// The array of AABBs parallel to the bodiesArray array specifying different AABBs to use for each body public static BuildCollisionLayerConfig BuildCollisionLayer(NativeArray bodies, NativeArray overrideAabbs) { - var config = new BuildCollisionLayerConfig(); - ValidateOverrideAabbsAreRightLength(overrideAabbs, bodies.Length, false); - - config.aabbs = overrideAabbs; - config.bodies = bodies; - config.hasAabbsArray = true; - config.hasBodiesArray = true; - config.settings = BuildCollisionLayerConfig.defaultSettings; - config.count = bodies.Length; + var config = new BuildCollisionLayerConfig + { + aabbsArray = overrideAabbs, + bodiesArray = bodies, + hasAabbsArray = true, + hasBodiesArray = true, + settings = BuildCollisionLayerConfig.defaultSettings + }; + return config; + } + + /// + /// Creates a new CollisionLayer using the collider and transform data provided by the bodies list. + /// If using a job scheduler, the list does not need to be populated at this time. + /// + /// The list of ColliderBody instances which should be baked into the CollisionLayer + public static BuildCollisionLayerConfig BuildCollisionLayer(NativeList bodies) + { + var config = new BuildCollisionLayerConfig + { + bodiesList = bodies, + hasBodiesArray = true, + usesLists = true, + settings = BuildCollisionLayerConfig.defaultSettings, + }; + return config; + } + + /// + /// Creates a new CollisionLayer using the collider and transform data provided by the bodies list, + /// but uses the AABBs provided by the overrideAabbs list instead of calculating AABBs from the bodies. + /// If using a job scheduler, the lists do not need to be populated at this time. + /// + /// The array of ColliderBody instances which should be baked into the CollisionLayer + /// The array of AABBs parallel to the bodiesArray array specifying different AABBs to use for each body + public static BuildCollisionLayerConfig BuildCollisionLayer(NativeList bodies, NativeList overrideAabbs) + { + //ValidateOverrideAabbsAreRightLength(overrideAabbs, bodies.Length, false); + + var config = new BuildCollisionLayerConfig + { + aabbsList = overrideAabbs, + bodiesList = bodies, + hasAabbsArray = true, + hasBodiesArray = true, + usesLists = true, + settings = BuildCollisionLayerConfig.defaultSettings + }; return config; } #endregion @@ -269,13 +314,24 @@ public static void RunImmediate(this BuildCollisionLayerConfig config, out Colli } else if (config.hasAabbsArray && config.hasBodiesArray) { - layer = new CollisionLayer(config.bodies.Length, config.settings, allocator); - BuildCollisionLayerInternal.BuildImmediate(ref layer, config.bodies, config.aabbs); + if (config.usesLists) + { + config.bodiesArray = config.bodiesList.AsArray(); + config.aabbsArray = config.aabbsList.AsArray(); + } + + layer = new CollisionLayer(config.settings, allocator); + BuildCollisionLayerInternal.BuildImmediate(ref layer, config.bodiesArray, config.aabbsArray); } else if (config.hasBodiesArray) { - layer = new CollisionLayer(config.bodies.Length, config.settings, allocator); - BuildCollisionLayerInternal.BuildImmediate(ref layer, config.bodies); + if (config.usesLists) + { + config.bodiesArray = config.bodiesList.AsArray(); + } + + layer = new CollisionLayer(config.settings, allocator); + BuildCollisionLayerInternal.BuildImmediate(ref layer, config.bodiesArray); } else { @@ -307,68 +363,50 @@ public static void Run(this BuildCollisionLayerConfig config, out CollisionLayer if (config.hasQueryData) { - int count = config.count; - layer = new CollisionLayer(count, config.settings, allocator); - var layerIndices = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - var aos = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - var xMinMaxs = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - - var part1Indices = config.query.CalculateBaseEntityIndexArray(Allocator.TempJob); - new BuildCollisionLayerInternal.Part1FromQueryJob + layer = new CollisionLayer(config.settings, allocator); + var filteredChunkCache = new NativeList(config.query.CalculateChunkCountWithoutFiltering(), Allocator.TempJob); + new BuildCollisionLayerInternal.Part0PrefilterQueryJob { - typeGroup = config.typeGroup, - layer = layer, - layerIndices = layerIndices, - colliderAoS = aos, - xMinMaxs = xMinMaxs, - firstEntityInChunkIndices = part1Indices + filteredChunkCache = filteredChunkCache }.Run(config.query); - - new BuildCollisionLayerInternal.Part2Job + new BuildCollisionLayerInternal.BuildFromFilteredChunkCacheSingleJob { - layer = layer, - layerIndices = layerIndices + filteredChunkCache = filteredChunkCache.AsArray(), + handles = config.typeGroup, + layer = layer }.Run(); - - new BuildCollisionLayerInternal.Part3Job - { - layerIndices = layerIndices, - unsortedSrcIndices = layer.srcIndices - }.Run(count); - - new BuildCollisionLayerInternal.Part4Job - { - bucketStartAndCounts = layer.bucketStartsAndCounts, - unsortedSrcIndices = layer.srcIndices, - trees = layer.intervalTrees, - xMinMaxs = xMinMaxs - }.Run(layer.bucketCount); - - new BuildCollisionLayerInternal.Part5FromQueryJob - { - colliderAoS = aos, - layer = layer, - }.Run(count); + filteredChunkCache.Dispose(); } else if (config.hasAabbsArray && config.hasBodiesArray) { - layer = new CollisionLayer(config.aabbs.Length, config.settings, allocator); + layer = new CollisionLayer(config.settings, allocator); + + if (config.usesLists) + { + config.bodiesArray = config.bodiesList.AsArray(); + config.aabbsArray = config.aabbsList.AsArray(); + } new BuildCollisionLayerInternal.BuildFromDualArraysSingleJob { layer = layer, - aabbs = config.aabbs, - bodies = config.bodies + aabbs = config.aabbsArray, + bodies = config.bodiesArray }.Run(); } else if (config.hasBodiesArray) { - layer = new CollisionLayer(config.bodies.Length, config.settings, allocator); + layer = new CollisionLayer(config.settings, allocator); + + if (config.usesLists) + { + config.bodiesArray = config.bodiesList.AsArray(); + } new BuildCollisionLayerInternal.BuildFromColliderArraySingleJob { layer = layer, - bodies = config.bodies + bodies = config.bodiesArray }.Run(); } else @@ -389,79 +427,53 @@ public static JobHandle ScheduleSingle(this BuildCollisionLayerConfig config, { config.ValidateSettings(); - var jh = inputDeps; - if (config.hasQueryData) { - int count = config.count; - layer = new CollisionLayer(count, config.settings, allocator); - var layerIndices = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - var aos = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - var xMinMaxs = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - - var part1Indices = config.query.CalculateBaseEntityIndexArrayAsync(Allocator.TempJob, jh, out jh); - jh = new BuildCollisionLayerInternal.Part1FromQueryJob - { - typeGroup = config.typeGroup, - layer = layer, - layerIndices = layerIndices, - colliderAoS = aos, - xMinMaxs = xMinMaxs, - firstEntityInChunkIndices = part1Indices - }.Schedule(config.query, jh); - - jh = new BuildCollisionLayerInternal.Part2Job - { - layer = layer, - layerIndices = layerIndices - }.Schedule(jh); - - jh = new BuildCollisionLayerInternal.Part3Job + layer = new CollisionLayer(config.settings, allocator); + var filteredChunkCache = new NativeList(config.query.CalculateChunkCountWithoutFiltering(), Allocator.TempJob); + var jh = new BuildCollisionLayerInternal.Part0PrefilterQueryJob { - layerIndices = layerIndices, - unsortedSrcIndices = layer.srcIndices - }.Schedule(jh); - - jh = new BuildCollisionLayerInternal.Part4Job + filteredChunkCache = filteredChunkCache + }.Schedule(config.query, inputDeps); + jh = new BuildCollisionLayerInternal.BuildFromFilteredChunkCacheSingleJob { - bucketStartAndCounts = layer.bucketStartsAndCounts, - unsortedSrcIndices = layer.srcIndices, - trees = layer.intervalTrees, - xMinMaxs = xMinMaxs + filteredChunkCache = filteredChunkCache.AsArray(), + handles = config.typeGroup, + layer = layer }.Schedule(jh); - - jh = new BuildCollisionLayerInternal.Part5FromQueryJob - { - colliderAoS = aos, - layer = layer, - }.Schedule(jh); - - return jh; + return filteredChunkCache.Dispose(jh); } else if (config.hasAabbsArray && config.hasBodiesArray) { - layer = new CollisionLayer(config.aabbs.Length, config.settings, allocator); + layer = new CollisionLayer(config.settings, allocator); - jh = new BuildCollisionLayerInternal.BuildFromDualArraysSingleJob + if (config.usesLists) { - layer = layer, - aabbs = config.aabbs, - bodies = config.bodies - }.Schedule(jh); + config.bodiesArray = config.bodiesList.AsDeferredJobArray(); + config.aabbsArray = config.aabbsList.AsDeferredJobArray(); + } - return jh; + return new BuildCollisionLayerInternal.BuildFromDualArraysSingleJob + { + layer = layer, + aabbs = config.aabbsArray, + bodies = config.bodiesArray + }.Schedule(inputDeps); } else if (config.hasBodiesArray) { - layer = new CollisionLayer(config.bodies.Length, config.settings, allocator); + layer = new CollisionLayer(config.settings, allocator); - jh = new BuildCollisionLayerInternal.BuildFromColliderArraySingleJob + if (config.usesLists) { - layer = layer, - bodies = config.bodies - }.Schedule(jh); + config.bodiesArray = config.bodiesList.AsDeferredJobArray(); + } - return jh; + return new BuildCollisionLayerInternal.BuildFromColliderArraySingleJob + { + layer = layer, + bodies = config.bodiesArray + }.Schedule(inputDeps); } else throw new InvalidOperationException("Something went wrong with the BuildCollisionError configuration."); @@ -481,117 +493,200 @@ public static JobHandle ScheduleParallel(this BuildCollisionLayerConfig config, { config.ValidateSettings(); + layer = new CollisionLayer(config.settings, allocator); + var jh = inputDeps; if (config.hasQueryData) { - int count = config.query.CalculateEntityCount(); - layer = new CollisionLayer(count, config.settings, allocator); - var layerIndices = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - var xMinMaxs = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - var aos = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - - var part1Indices = config.query.CalculateBaseEntityIndexArrayAsync(Allocator.TempJob, jh, out jh); - jh = new BuildCollisionLayerInternal.Part1FromQueryJob + NativeList layerIndices; + NativeList xMinMaxs; + NativeList aos; + + JobHandle filteredCacheDisposeHandle = default; + + if (config.query.HasFilter() || config.query.UsesEnabledFiltering()) + { + var filteredChunkCache = new NativeList(config.query.CalculateChunkCountWithoutFiltering(), Allocator.TempJob); + jh = new BuildCollisionLayerInternal.Part0PrefilterQueryJob + { + filteredChunkCache = filteredChunkCache + }.Schedule(config.query, jh); + + layerIndices = new NativeList(Allocator.TempJob); + xMinMaxs = new NativeList(Allocator.TempJob); + aos = new NativeList(Allocator.TempJob); + + jh = new BuildCollisionLayerInternal.AllocateCollisionLayerFromFilteredQueryJob + { + layer = layer, + filteredChunkCache = filteredChunkCache.AsDeferredJobArray(), + colliderAoS = aos, + layerIndices = layerIndices, + xMinMaxs = xMinMaxs + }.Schedule(jh); + + jh = new BuildCollisionLayerInternal.Part1FromFilteredQueryJob + { + layer = layer, + filteredChunkCache = filteredChunkCache.AsDeferredJobArray(), + colliderAoS = aos.AsDeferredJobArray(), + layerIndices = layerIndices.AsDeferredJobArray(), + typeGroup = config.typeGroup, + xMinMaxs = xMinMaxs.AsDeferredJobArray() + }.Schedule(filteredChunkCache, 1, jh); + + filteredCacheDisposeHandle = filteredChunkCache.Dispose(jh); + } + else { - layer = layer, - typeGroup = config.typeGroup, - layerIndices = layerIndices, - xMinMaxs = xMinMaxs, - colliderAoS = aos, - firstEntityInChunkIndices = part1Indices - }.ScheduleParallel(config.query, jh); + int count = config.query.CalculateEntityCountWithoutFiltering(); + layerIndices = new NativeList(count, Allocator.TempJob); + layerIndices.ResizeUninitialized(count); + xMinMaxs = new NativeList(count, Allocator.TempJob); + xMinMaxs.ResizeUninitialized(count); + aos = new NativeList(count, Allocator.TempJob); + aos.ResizeUninitialized(count); + layer.ResizeUninitialized(count); + var part1Indices = config.query.CalculateBaseEntityIndexArrayAsync(Allocator.TempJob, jh, out jh); + + jh = new BuildCollisionLayerInternal.Part1FromUnfilteredQueryJob + { + layer = layer, + typeGroup = config.typeGroup, + layerIndices = layerIndices.AsDeferredJobArray(), + xMinMaxs = xMinMaxs.AsDeferredJobArray(), + colliderAoS = aos.AsDeferredJobArray(), + firstEntityInChunkIndices = part1Indices + }.ScheduleParallel(config.query, jh); + } jh = new BuildCollisionLayerInternal.Part2Job { layer = layer, - layerIndices = layerIndices + layerIndices = layerIndices.AsDeferredJobArray() }.Schedule(jh); + if (filteredCacheDisposeHandle != default) + jh = JobHandle.CombineDependencies(filteredCacheDisposeHandle, jh); + jh = new BuildCollisionLayerInternal.Part3Job { - layerIndices = layerIndices, - unsortedSrcIndices = layer.srcIndices - }.Schedule(count, 512, jh); + layerIndices = layerIndices.AsDeferredJobArray(), + unsortedSrcIndices = layer.srcIndices.AsDeferredJobArray() + }.Schedule(layerIndices, 512, jh); jh = new BuildCollisionLayerInternal.Part4Job { - unsortedSrcIndices = layer.srcIndices, - xMinMaxs = xMinMaxs, - trees = layer.intervalTrees, - bucketStartAndCounts = layer.bucketStartsAndCounts + unsortedSrcIndices = layer.srcIndices.AsDeferredJobArray(), + xMinMaxs = xMinMaxs.AsDeferredJobArray(), + trees = layer.intervalTrees.AsDeferredJobArray(), + bucketStartAndCounts = layer.bucketStartsAndCounts.AsDeferredJobArray() }.Schedule(layer.bucketCount, 1, jh); - jh = new BuildCollisionLayerInternal.Part5FromQueryJob + jh = new BuildCollisionLayerInternal.Part5FromAoSJob { layer = layer, - colliderAoS = aos, - }.Schedule(count, 128, jh); + colliderAoS = aos.AsDeferredJobArray(), + }.Schedule(aos, 128, jh); - return jh; + return JobHandle.CombineDependencies(layerIndices.Dispose(jh), xMinMaxs.Dispose(jh), aos.Dispose(jh)); } else if (config.hasBodiesArray) { - layer = new CollisionLayer(config.bodies.Length, config.settings, allocator); - int count = config.bodies.Length; - var layerIndices = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - var xMinMaxs = new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); - - NativeArray aabbs = config.hasAabbsArray ? config.aabbs : new NativeArray(count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + NativeList layerIndices; + NativeList xMinMaxs; + if (config.usesLists) + { + if (!config.hasAabbsArray) + { + config.aabbsList = new NativeList(Allocator.TempJob); + } + layerIndices = new NativeList(Allocator.TempJob); + xMinMaxs = new NativeList(Allocator.TempJob); + jh = new BuildCollisionLayerInternal.AllocateCollisionLayerFromBodiesListJob + { + aabbs = config.aabbsList, + aabbsAreProvided = config.hasAabbsArray, + bodies = config.bodiesList.AsDeferredJobArray(), + layer = layer, + layerIndices = layerIndices, + xMinMaxs = xMinMaxs + }.Schedule(jh); + config.aabbsArray = config.aabbsList.AsDeferredJobArray(); + config.bodiesArray = config.bodiesList.AsDeferredJobArray(); + } + else + { + int count = config.bodiesArray.Length; + if (!config.hasAabbsArray) + { + config.aabbsList = new NativeList(count, Allocator.TempJob); + config.aabbsList.ResizeUninitialized(count); + config.aabbsArray = config.aabbsList.AsDeferredJobArray(); + } + layerIndices = new NativeList(count, Allocator.TempJob); + layerIndices.ResizeUninitialized(count); + xMinMaxs = new NativeList(count, Allocator.TempJob); + xMinMaxs.ResizeUninitialized(count); + layer.ResizeUninitialized(count); + } if (config.hasAabbsArray) { jh = new BuildCollisionLayerInternal.Part1FromDualArraysJob { layer = layer, - aabbs = aabbs, - layerIndices = layerIndices, - xMinMaxs = xMinMaxs - }.Schedule(count, 64, jh); + aabbs = config.aabbsArray, + layerIndices = layerIndices.AsDeferredJobArray(), + xMinMaxs = xMinMaxs.AsDeferredJobArray() + }.Schedule(layerIndices, 64, jh); } else { jh = new BuildCollisionLayerInternal.Part1FromColliderBodyArrayJob { layer = layer, - aabbs = aabbs, - colliderBodies = config.bodies, - layerIndices = layerIndices, - xMinMaxs = xMinMaxs - }.Schedule(count, 64, jh); + aabbs = config.aabbsArray, + colliderBodies = config.bodiesArray, + layerIndices = layerIndices.AsDeferredJobArray(), + xMinMaxs = xMinMaxs.AsDeferredJobArray() + }.Schedule(layerIndices, 64, jh); } jh = new BuildCollisionLayerInternal.Part2Job { layer = layer, - layerIndices = layerIndices + layerIndices = layerIndices.AsDeferredJobArray() }.Schedule(jh); jh = new BuildCollisionLayerInternal.Part3Job { - layerIndices = layerIndices, - unsortedSrcIndices = layer.srcIndices - }.Schedule(count, 512, jh); + layerIndices = layerIndices.AsDeferredJobArray(), + unsortedSrcIndices = layer.srcIndices.AsDeferredJobArray() + }.Schedule(layerIndices, 512, jh); jh = new BuildCollisionLayerInternal.Part4Job { - bucketStartAndCounts = layer.bucketStartsAndCounts, - unsortedSrcIndices = layer.srcIndices, - trees = layer.intervalTrees, - xMinMaxs = xMinMaxs + bucketStartAndCounts = layer.bucketStartsAndCounts.AsDeferredJobArray(), + unsortedSrcIndices = layer.srcIndices.AsDeferredJobArray(), + trees = layer.intervalTrees.AsDeferredJobArray(), + xMinMaxs = xMinMaxs.AsDeferredJobArray() }.Schedule(layer.bucketCount, 1, jh); - jh = new BuildCollisionLayerInternal.Part5FromArraysJob + jh = new BuildCollisionLayerInternal.Part5FromSplitArraysJob { - aabbs = aabbs, - bodies = config.bodies, + aabbs = config.aabbsArray, + bodies = config.bodiesArray, layer = layer, - }.Schedule(count, 128, jh); + }.Schedule(layer.bodies, 128, jh); + JobHandle aabbDispose = default; if (!config.hasAabbsArray) - jh = aabbs.Dispose(jh); - - return jh; + { + aabbDispose = config.aabbsList.Dispose(jh); + } + return JobHandle.CombineDependencies(aabbDispose, xMinMaxs.Dispose(jh), layerIndices.Dispose(jh)); } else throw new InvalidOperationException("Something went wrong with the BuildCollisionError configuration."); @@ -608,13 +703,6 @@ private static void ValidateSettings(this BuildCollisionLayerConfig config) throw new InvalidOperationException("BuildCollisionLayer requires positive Subdivision values per axis"); } - [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] - private static void ValidateOverrideAabbsAreRightLength(NativeArray aabbs, int count, bool query) - { - if (aabbs.Length != count) - throw new InvalidOperationException( - $"The number of elements in overrideAbbs does not match the { (query ? "number of entities in the query" : "number of bodies in the bodies array")}"); - } #endregion } } diff --git a/PsyshockPhysics/Physics/Spatial/Modifiers/Physics.ScaleCollider.cs b/PsyshockPhysics/Physics/Spatial/Modifiers/Physics.ScaleCollider.cs index bab9bdf..b2dfc53 100644 --- a/PsyshockPhysics/Physics/Spatial/Modifiers/Physics.ScaleCollider.cs +++ b/PsyshockPhysics/Physics/Spatial/Modifiers/Physics.ScaleCollider.cs @@ -22,28 +22,28 @@ public static void ScaleStretchCollider(ref Collider collider, float scale, floa switch (collider.type) { case ColliderType.Sphere: - ScaleStretchCollider(ref collider.m_sphere, scale, stretch); + ScaleStretchCollider(ref collider.m_sphere, scale, stretch); break; case ColliderType.Capsule: - ScaleStretchCollider(ref collider.m_capsule, scale, stretch); + ScaleStretchCollider(ref collider.m_capsule, scale, stretch); break; case ColliderType.Box: - ScaleStretchCollider(ref collider.m_box, scale, stretch); + ScaleStretchCollider(ref collider.m_box, scale, stretch); break; case ColliderType.Triangle: - ScaleStretchCollider(ref collider.m_triangle, scale, stretch); + ScaleStretchCollider(ref collider.m_triangle, scale, stretch); break; case ColliderType.Convex: - ScaleStretchCollider(ref collider.m_convex, scale, stretch); + ScaleStretchCollider(ref collider.m_convex, scale, stretch); break; case ColliderType.TriMesh: - ScaleStretchCollider(ref collider.m_triMesh, scale, stretch); + ScaleStretchCollider(ref collider.m_triMeshRW(), scale, stretch); break; case ColliderType.Compound: - ScaleStretchCollider(ref collider.m_compound, scale, stretch); + ScaleStretchCollider(ref collider.m_compoundRW(), scale, stretch); break; default: - ThrowUnsupportedType(); + ThrowUnsupportedType(collider.type); break; } } diff --git a/PsyshockPhysics/Physics/Spatial/Queries/Physics.Aabbs.cs b/PsyshockPhysics/Physics/Spatial/Queries/Physics.Aabbs.cs index 2c33afd..183ee63 100644 --- a/PsyshockPhysics/Physics/Spatial/Queries/Physics.Aabbs.cs +++ b/PsyshockPhysics/Physics/Spatial/Queries/Physics.Aabbs.cs @@ -54,11 +54,11 @@ public static Aabb AabbFrom(in Collider collider, in TransformQvvs transform) case ColliderType.Convex: return AabbFrom(collider.m_convex, in transform); case ColliderType.TriMesh: - return AabbFrom(collider.m_triMesh, in transform); + return AabbFrom(collider.m_triMesh(), in transform); case ColliderType.Compound: - return AabbFrom(collider.m_compound, in transform); + return AabbFrom(collider.m_compound(), in transform); default: - ThrowUnsupportedType(); + ThrowUnsupportedType(collider.type); return new Aabb(); } } @@ -93,11 +93,11 @@ public static Aabb AabbFrom(in Collider colliderToCast, in TransformQvvs castSta case ColliderType.Convex: return AabbFrom(colliderToCast.m_convex, in castStart, castEnd); case ColliderType.TriMesh: - return AabbFrom(colliderToCast.m_triMesh, in castStart, castEnd); + return AabbFrom(colliderToCast.m_triMesh(), in castStart, castEnd); case ColliderType.Compound: - return AabbFrom(colliderToCast.m_compound, in castStart, castEnd); + return AabbFrom(colliderToCast.m_compound(), in castStart, castEnd); default: - ThrowUnsupportedType(); + ThrowUnsupportedType(colliderToCast.type); return new Aabb(); } } @@ -117,11 +117,11 @@ internal static Aabb AabbFrom(in Collider collider, in RigidTransform transform) case ColliderType.Convex: return AabbFrom(collider.m_convex, in transform); case ColliderType.TriMesh: - return AabbFrom(collider.m_triMesh, in transform); + return AabbFrom(collider.m_triMesh(), in transform); case ColliderType.Compound: - return AabbFrom(collider.m_compound, in transform); + return AabbFrom(collider.m_compound(), in transform); default: - ThrowUnsupportedType(); + ThrowUnsupportedType(collider.type); return new Aabb(); } } @@ -141,17 +141,17 @@ internal static Aabb AabbFrom(in Collider colliderToCast, in RigidTransform cast case ColliderType.Convex: return AabbFrom(colliderToCast.m_convex, in castStart, castEnd); case ColliderType.Compound: - return AabbFrom(colliderToCast.m_compound, in castStart, castEnd); + return AabbFrom(colliderToCast.m_compound(), in castStart, castEnd); default: - ThrowUnsupportedType(); + ThrowUnsupportedType(colliderToCast.type); return new Aabb(); } } [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] - private static void ThrowUnsupportedType() + private static void ThrowUnsupportedType(ColliderType type) { - throw new InvalidOperationException("Collider type not supported yet"); + throw new InvalidOperationException($"Collider type not supported yet. Type code is {(int)type}"); } #endregion diff --git a/PsyshockPhysics/Physics/Spatial/Queries/Physics.DistanceBetween.cs b/PsyshockPhysics/Physics/Spatial/Queries/Physics.DistanceBetween.cs index 3dace39..3dd03fa 100644 --- a/PsyshockPhysics/Physics/Spatial/Queries/Physics.DistanceBetween.cs +++ b/PsyshockPhysics/Physics/Spatial/Queries/Physics.DistanceBetween.cs @@ -144,7 +144,8 @@ public static bool DistanceBetween(in Collider colliderA, /// The transform of the second of the two colliders /// The signed distance the surface points must be less than in order for a "hit" to be registered. A value less than 0 requires that /// the colliders be overlapping. - /// The processor that will receive a ColliderDistanceResult for each found pair of subcolliders. + /// The processor that will receive a ColliderDistanceResult for each found pair of subcolliders. + /// Use DistanceBetweenAllCache if you need a simple collector. public static void DistanceBetweenAll(in Collider colliderA, in TransformQvvs transformA, in Collider colliderB, @@ -155,14 +156,22 @@ public static void DistanceBetweenAll(in Collider colliderA, var scaledColliderA = colliderA; var scaledColliderB = colliderB; + var context = new DistanceBetweenAllContext + { + numSubcollidersA = InternalQueryTypeUtilities.GetSubcolliders(in colliderA), + numSubcollidersB = InternalQueryTypeUtilities.GetSubcolliders(in colliderB) + }; + ScaleStretchCollider(ref scaledColliderA, transformA.scale, transformA.stretch); ScaleStretchCollider(ref scaledColliderB, transformB.scale, transformB.stretch); + processor.Begin(in context); ColliderColliderDispatch.DistanceBetweenAll(in scaledColliderA, new RigidTransform(transformA.rotation, transformA.position), in scaledColliderB, new RigidTransform(transformB.rotation, transformB.position), maxDistance, ref processor); + processor.End(in context); } #endregion @@ -269,7 +278,33 @@ public static bool DistanceBetweenAny(Collider collider, /// public interface IDistanceBetweenAllProcessor { + /// + /// Called whenever a pair is found between any two subcolliders from each collider + /// void Execute(in ColliderDistanceResult result); + + /// + /// Called before any Execute() call. Used to initialize or reset resources. + /// + public void Begin(in DistanceBetweenAllContext context) + { + } + + /// + /// Called after all Execute() calls. Used to finalize and reorganize any backing resources. + /// + public void End(in DistanceBetweenAllContext context) + { + } + } + + /// + /// A context used to provide additional info about the colliders being processed in a DistanceBetweenAll() operation. + /// + public struct DistanceBetweenAllContext + { + public int numSubcollidersA; + public int numSubcollidersB; } #endregion } diff --git a/PsyshockPhysics/Physics/Spatial/Queries/Physics.FindObjects.cs b/PsyshockPhysics/Physics/Spatial/Queries/Physics.FindObjects.cs index 15dd1b8..8f8cc80 100644 --- a/PsyshockPhysics/Physics/Spatial/Queries/Physics.FindObjects.cs +++ b/PsyshockPhysics/Physics/Spatial/Queries/Physics.FindObjects.cs @@ -69,14 +69,14 @@ public struct FindObjectsResult #if ENABLE_UNITY_COLLECTIONS_CHECKS public SafeEntity entity => new SafeEntity { - entity = new Entity + m_entity = new Entity { Index = math.select(-body.entity.Index - 1, body.entity.Index, m_isThreadSafe), Version = body.entity.Version } }; #else - public SafeEntity entity => new SafeEntity { entity = body.entity }; + public SafeEntity entity => new SafeEntity { m_entity = body.entity }; #endif /// @@ -183,7 +183,7 @@ public T RunImmediate() /// public void Run() { - new FindObjectsInternal.Single + new FindObjectsInternal.SingleJob { layer = layer, processor = processor, @@ -198,7 +198,7 @@ public void Run() /// The JobHandle of the scheduled job public JobHandle ScheduleSingle(JobHandle inputDeps = default) { - return new FindObjectsInternal.Single + return new FindObjectsInternal.SingleJob { layer = layer, processor = processor, diff --git a/PsyshockPhysics/Physics/Spatial/Queries/Physics.FindPairs.cs b/PsyshockPhysics/Physics/Spatial/Queries/Physics.FindPairs.cs index a86db01..8f2cf96 100644 --- a/PsyshockPhysics/Physics/Spatial/Queries/Physics.FindPairs.cs +++ b/PsyshockPhysics/Physics/Spatial/Queries/Physics.FindPairs.cs @@ -7,7 +7,6 @@ using Unity.Jobs; using Unity.Mathematics; -//Todo: FilteredCache playback and inflations namespace Latios.Psyshock { /// @@ -23,8 +22,11 @@ public interface IFindPairsProcessor /// An optional callback prior to processing a bucket (single layer) or bucket combination (two layers). /// Each invocation of this callback will have a different jobIndex. /// - void BeginBucket(in FindPairsBucketContext context) + /// Returns true if the Execute() and EndBucket() methods should be called. Otherwise, further + /// processing of the bucket is skipped. + bool BeginBucket(in FindPairsBucketContext context) { + return true; } /// /// An optional callback following processing a bucket (single layer) or bucket combination (two layers). @@ -101,54 +103,16 @@ public struct FindPairsResult /// public int jobIndex => m_jobIndex; - private CollisionLayer m_layerA; - private CollisionLayer m_layerB; - private int m_bucketStartA; - private int m_bucketStartB; - private int m_bodyAIndex; - private int m_bodyBIndex; - private int m_jobIndex; - private bool m_isThreadSafe; - -#if ENABLE_UNITY_COLLECTIONS_CHECKS - /// - /// A safe entity handle that can be used inside of PhysicsComponentLookup or PhysicsBufferLookup and corresponds to the - /// owning entity of the first collider in the pair. It can also be implicitly casted and used as a normal entity reference. - /// - public SafeEntity entityA => new SafeEntity - { - entity = new Entity - { - Index = math.select(-bodyA.entity.Index - 1, bodyA.entity.Index, m_isThreadSafe), - Version = bodyA.entity.Version - } - }; - /// - /// A safe entity handle that can be used inside of PhysicsComponentLookup or PhysicsBufferLookup and corresponds to the - /// owning entity of the second collider in the pair. It can also be implicitly casted and used as a normal entity reference. - /// - public SafeEntity entityB => new SafeEntity - { - entity = new Entity - { - Index = math.select(-bodyB.entity.Index - 1, bodyB.entity.Index, m_isThreadSafe), - Version = bodyB.entity.Version - } - }; -#else - public SafeEntity entityA => new SafeEntity { entity = bodyA.entity }; - public SafeEntity entityB => new SafeEntity { entity = bodyB.entity }; -#endif - /// /// The Aabb of the first collider in the pair /// public Aabb aabbA { - get { + get + { var yzminmax = m_layerA.yzminmaxs[m_bodyAIndex]; - var xmin = m_layerA.xmins[ m_bodyAIndex]; - var xmax = m_layerA.xmaxs[ m_bodyAIndex]; + var xmin = m_layerA.xmins[m_bodyAIndex]; + var xmax = m_layerA.xmaxs[m_bodyAIndex]; return new Aabb(new float3(xmin, yzminmax.xy), new float3(xmax, -yzminmax.zw)); } } @@ -161,36 +125,125 @@ public Aabb aabbB get { var yzminmax = m_layerB.yzminmaxs[m_bodyBIndex]; - var xmin = m_layerB.xmins[ m_bodyBIndex]; - var xmax = m_layerB.xmaxs[ m_bodyBIndex]; + var xmin = m_layerB.xmins[m_bodyBIndex]; + var xmax = m_layerB.xmaxs[m_bodyBIndex]; return new Aabb(new float3(xmin, yzminmax.xy), new float3(xmax, -yzminmax.zw)); } } - internal FindPairsResult(in CollisionLayer layerA, in CollisionLayer layerB, in BucketSlices bucketA, in BucketSlices bucketB, int jobIndex, bool isThreadSafe) + /// + /// A key that can be used in a PairStream.ParallelWriter + /// + public PairStream.ParallelWriteKey pairStreamKey { - m_layerA = layerA; - m_layerB = layerB; - m_bucketStartA = bucketA.bucketGlobalStart; - m_bucketStartB = bucketB.bucketGlobalStart; - m_jobIndex = jobIndex; - m_isThreadSafe = isThreadSafe; - m_bodyAIndex = 0; - m_bodyBIndex = 0; + get + { + int factor = 1; + if (jobIndex >= layerA.bucketCount) + factor++; + if (jobIndex >= 2 * layerA.bucketCount - 1) + factor++; + return new PairStream.ParallelWriteKey + { + entityA = bodyA.entity, + entityB = bodyB.entity, + streamIndexA = m_bucketIndexA * factor, + streamIndexB = m_bucketIndexB * factor, + streamIndexCombined = 2 * layerA.bucketCount + jobIndex, + expectedBucketCount = layerA.bucketCount + }; + } } - internal static FindPairsResult CreateGlobalResult(in CollisionLayer layerA, in CollisionLayer layerB, int jobIndex, bool isThreadSafe) +#if ENABLE_UNITY_COLLECTIONS_CHECKS + /// + /// A safe entity handle that can be used inside of PhysicsComponentLookup or PhysicsBufferLookup and corresponds to the + /// owning entity of the first collider in the pair. It can also be implicitly casted and used as a normal entity reference. + /// + public SafeEntity entityA => new SafeEntity + { + m_entity = new Entity + { + Index = math.select(-bodyA.entity.Index - 1, bodyA.entity.Index, m_isAThreadSafe), + Version = bodyA.entity.Version + } + }; + /// + /// A safe entity handle that can be used inside of PhysicsComponentLookup or PhysicsBufferLookup and corresponds to the + /// owning entity of the second collider in the pair. It can also be implicitly casted and used as a normal entity reference. + /// + public SafeEntity entityB => new SafeEntity + { + m_entity = new Entity + { + Index = math.select(-bodyB.entity.Index - 1, bodyB.entity.Index, m_isBThreadSafe), + Version = bodyB.entity.Version + } + }; +#else + public SafeEntity entityA => new SafeEntity { m_entity = bodyA.entity }; + public SafeEntity entityB => new SafeEntity { m_entity = bodyB.entity }; +#endif + + private CollisionLayer m_layerA; + private CollisionLayer m_layerB; + private int m_bucketStartA; + private int m_bucketStartB; + private int m_bodyAIndex; + private int m_bodyBIndex; + private int m_bucketIndexA; + private int m_bucketIndexB; + private int m_jobIndex; + private bool m_isAThreadSafe; + private bool m_isBThreadSafe; + private bool m_isImmediateContext; + + internal FindPairsResult(in CollisionLayer layerA, + in CollisionLayer layerB, + in BucketSlices bucketA, + in BucketSlices bucketB, + int jobIndex, + bool isAThreadSafe, + bool isBThreadSafe, + bool isImmediateContext = false) + { + m_layerA = layerA; + m_layerB = layerB; + m_bucketStartA = bucketA.bucketGlobalStart; + m_bucketStartB = bucketB.bucketGlobalStart; + m_bucketIndexA = bucketA.bucketIndex; + m_bucketIndexB = bucketB.bucketIndex; + m_jobIndex = jobIndex; + m_isAThreadSafe = isAThreadSafe; + m_isBThreadSafe = isBThreadSafe; + m_isImmediateContext = isImmediateContext; + m_bodyAIndex = 0; + m_bodyBIndex = 0; + } + + internal static FindPairsResult CreateGlobalResult(in CollisionLayer layerA, + in CollisionLayer layerB, + int bucketIndexA, + int bucketIndexB, + int jobIndex, + bool isAThreadSafe, + bool isBThreadSafe, + bool isImmediateContext = false) { return new FindPairsResult { - m_layerA = layerA, - m_layerB = layerB, - m_bucketStartA = 0, - m_bucketStartB = 0, - m_jobIndex = jobIndex, - m_isThreadSafe = isThreadSafe, - m_bodyAIndex = 0, - m_bodyBIndex = 0, + m_layerA = layerA, + m_layerB = layerB, + m_bucketStartA = 0, + m_bucketStartB = 0, + m_bucketIndexA = bucketIndexA, + m_bucketIndexB = bucketIndexB, + m_jobIndex = jobIndex, + m_isAThreadSafe = isAThreadSafe, + m_isBThreadSafe = isBThreadSafe, + m_isImmediateContext = isImmediateContext, + m_bodyAIndex = 0, + m_bodyBIndex = 0, }; } @@ -200,6 +253,13 @@ internal void SetBucketRelativePairIndices(int aIndex, int bIndex) m_bodyBIndex = bIndex + m_bucketStartB; } //Todo: Shorthands for calling narrow phase distance and manifold queries + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckCanGenerateParallelPairKey() + { + if (m_isImmediateContext) + throw new InvalidOperationException($"Cannot generate a ParallelWriteKey in a FindPairs.RunImmediate() context."); + } } /// @@ -242,6 +302,31 @@ public struct FindPairsBucketContext /// public int jobIndex => m_jobIndex; + /// + /// A key that can be used in a PairStream.ParallelWriter + /// + public PairStream.ParallelWriteKey CreatePairStreamKey(int aIndex, int bIndex) + { + CheckSafeEntityInRange(aIndex, bucketStartA, bucketCountA); + CheckSafeEntityInRange(bIndex, bucketStartB, bucketCountB); + CheckCanGenerateParallelPairKey(); + + int factor = 1; + if (jobIndex >= layerA.bucketCount) + factor++; + if (jobIndex >= 2 * layerA.bucketCount - 1) + factor++; + return new PairStream.ParallelWriteKey + { + entityA = layerA.bodies[aIndex].entity, + entityB = layerB.bodies[bIndex].entity, + streamIndexA = m_bucketIndexA * factor, + streamIndexB = m_bucketIndexB * factor, + streamIndexCombined = 2 * layerA.bucketCount + jobIndex, + expectedBucketCount = layerA.bucketCount + }; + } + /// /// Obtains a safe entity handle that can be used inside of PhysicsComponentLookup or PhysicsBufferLookup and corresponds to the /// owning entity of the first collider in any pair in this bucket for layerA. It can also be implicitly casted and used as a normal @@ -254,15 +339,16 @@ public SafeEntity GetSafeEntityInA(int aIndex) var entity = layerA.bodies[aIndex].entity; return new SafeEntity { - entity = new Entity + m_entity = new Entity { - Index = math.select(-entity.Index - 1, entity.Index, m_isThreadSafe), + Index = math.select(-entity.Index - 1, entity.Index, m_isAThreadSafe), Version = entity.Version } }; #else - return new SafeEntity { - entity = layerA.bodies[aIndex].entity + return new SafeEntity + { + m_entity = layerA.bodies[aIndex].entity }; #endif } @@ -279,15 +365,16 @@ public SafeEntity GetSafeEntityInB(int bIndex) var entity = layerB.bodies[bIndex].entity; return new SafeEntity { - entity = new Entity + m_entity = new Entity { - Index = math.select(-entity.Index - 1, entity.Index, m_isThreadSafe), + Index = math.select(-entity.Index - 1, entity.Index, m_isBThreadSafe), Version = entity.Version } }; #else - return new SafeEntity { - entity = layerB.bodies[bIndex].entity + return new SafeEntity + { + m_entity = layerB.bodies[bIndex].entity }; #endif } @@ -298,19 +385,34 @@ public SafeEntity GetSafeEntityInB(int bIndex) private int m_bucketStartB; private int m_bucketCountA; private int m_bucketCountB; + private int m_bucketIndexA; + private int m_bucketIndexB; private int m_jobIndex; - private bool m_isThreadSafe; - - internal FindPairsBucketContext(in CollisionLayer layerA, in CollisionLayer layerB, int startA, int countA, int startB, int countB, int jobIndex, bool isThreadSafe) - { - m_layerA = layerA; - m_layerB = layerB; - m_bucketStartA = startA; - m_bucketCountA = countA; - m_bucketStartB = startB; - m_bucketCountB = countB; - m_jobIndex = jobIndex; - m_isThreadSafe = isThreadSafe; + private bool m_isAThreadSafe; + private bool m_isBThreadSafe; + private bool m_isImmediateContext; + + internal FindPairsBucketContext(in CollisionLayer layerA, + in CollisionLayer layerB, + in BucketSlices bucketA, + in BucketSlices bucketB, + int jobIndex, + bool isAThreadSafe, + bool isBThreadSafe, + bool isImmediateContext = false) + { + m_layerA = layerA; + m_layerB = layerB; + m_bucketStartA = bucketA.bucketGlobalStart; + m_bucketCountA = bucketA.count; + m_bucketIndexA = bucketA.bucketIndex; + m_bucketStartB = bucketB.bucketGlobalStart; + m_bucketCountB = bucketB.count; + m_bucketIndexB = bucketB.bucketIndex; + m_jobIndex = jobIndex; + m_isAThreadSafe = isAThreadSafe; + m_isBThreadSafe = isBThreadSafe; + m_isImmediateContext = isImmediateContext; } [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] @@ -320,6 +422,13 @@ void CheckSafeEntityInRange(int index, int start, int count) if (clampedIndex != index) throw new ArgumentOutOfRangeException($"Index {index} is outside the bucket range of [{start}, {start + count - 1}]."); } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckCanGenerateParallelPairKey() + { + if (m_isImmediateContext) + throw new InvalidOperationException($"Cannot generate a ParallelWriteKey in a FindPairs.RunImmediate() context."); + } } public static partial class Physics @@ -380,42 +489,8 @@ public static FindPairsLayerLayerConfig FindPairs(in CollisionLayer layerA /// Every jobIndex will be less than this value. public static int FindPairsJobIndexCount(in CollisionLayer layerA, in CollisionLayer layerB) => 3 * layerA.bucketCount - 2; - /// - /// Request a FindPairs broadphase operation to report pairs within the layer. - /// This is the start of a fluent expression. - /// - /// The layer in which pairs should be detected - /// The job-like struct which should process each pair found - internal static FindPairsLayerSelfConfigUnrolled FindPairsUnrolled(in CollisionLayer layer, in T processor) where T : struct, IFindPairsProcessor - { - return new FindPairsLayerSelfConfigUnrolled - { - processor = processor, - layer = layer, - disableEntityAliasChecks = false - }; - } - - /// - /// Request a FindPairs broadphase operation to report pairs between the two layers. - /// Only pairs containing one element from layerA and one element from layerB will be reported. - /// This is the start of a fluent expression. - /// - /// The first layer in which pairs should be detected - /// The second layer in which pairs should be detected - /// The job-like struct which should process each pair found - internal static FindPairsLayerLayerConfigUnrolled FindPairsUnrolled(in CollisionLayer layerA, in CollisionLayer layerB, in T processor) where T : struct, - IFindPairsProcessor - { - CheckLayersAreCompatible(layerA, layerB); - return new FindPairsLayerLayerConfigUnrolled - { - processor = processor, - layerA = layerA, - layerB = layerB, - disableEntityAliasChecks = false - }; - } + internal static readonly Unity.Profiling.ProfilerMarker kCellMarker = new Unity.Profiling.ProfilerMarker("Cell"); + internal static readonly Unity.Profiling.ProfilerMarker kCrossMarker = new Unity.Profiling.ProfilerMarker("Cross"); #region SafetyChecks [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] @@ -430,6 +505,18 @@ static void CheckLayersAreCompatible(in CollisionLayer layerA, in CollisionLayer #endif } } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + internal static void WarnEntityAliasingUnchecked() + { + UnityEngine.Debug.LogWarning("IgnoreEntityAliasing is unchecked for this schedule mode."); + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + internal static void WarnCrossCacheUnused() + { + UnityEngine.Debug.LogWarning("Cross-caching is unsupported for this schedule mode at this time. The setting is being ignored."); + } #endregion } @@ -440,6 +527,7 @@ public partial struct FindPairsLayerSelfConfig where T : struct, IFindPairsPr internal CollisionLayer layer; internal bool disableEntityAliasChecks; + internal bool useCrossCache; #region Settings /// @@ -450,23 +538,6 @@ public FindPairsLayerSelfConfig WithoutEntityAliasingChecks() disableEntityAliasChecks = true; return this; } - - /// - /// Enables usage of a cache for pairs involving the cross bucket. - /// This increases processing time and memory usage, but may decrease latency. - /// This may only be used with ScheduleParallel(). - /// - /// The type of allocator to use for the cache - public FindPairsLayerSelfWithCrossCacheConfig WithCrossCache(Allocator cacheAllocator = Allocator.TempJob) - { - return new FindPairsLayerSelfWithCrossCacheConfig - { - layer = layer, - disableEntityAliasChecks = disableEntityAliasChecks, - processor = processor, - allocator = cacheAllocator - }; - } #endregion #region Schedulers @@ -475,6 +546,14 @@ public FindPairsLayerSelfWithCrossCacheConfig WithCrossCache(Allocator cacheA /// public void RunImmediate() { + if (disableEntityAliasChecks) + { + Physics.WarnEntityAliasingUnchecked(); + } + if (useCrossCache) + { + Physics.WarnCrossCacheUnused(); + } FindPairsInternal.RunImmediate(layer, ref processor, false); } @@ -483,11 +562,15 @@ public void RunImmediate() /// public void Run() { - new FindPairsInternal.LayerSelfSingle + if (disableEntityAliasChecks) + { + Physics.WarnEntityAliasingUnchecked(); + } + if (useCrossCache) { - layer = layer, - processor = processor - }.Run(); + Physics.WarnCrossCacheUnused(); + } + new FindPairsInternal.LayerSelfJob(in layer, in processor).Run(); } /// @@ -497,11 +580,15 @@ public void Run() /// A JobHandle for the scheduled job public JobHandle ScheduleSingle(JobHandle inputDeps = default) { - return new FindPairsInternal.LayerSelfSingle + if (disableEntityAliasChecks) + { + Physics.WarnEntityAliasingUnchecked(); + } + if (useCrossCache) { - layer = layer, - processor = processor - }.Schedule(inputDeps); + Physics.WarnCrossCacheUnused(); + } + return new FindPairsInternal.LayerSelfJob(in layer, in processor).ScheduleSingle(inputDeps); } /// @@ -511,36 +598,14 @@ public JobHandle ScheduleSingle(JobHandle inputDeps = default) /// The final JobHandle for the scheduled jobs public JobHandle ScheduleParallel(JobHandle inputDeps = default) { - JobHandle jh = new FindPairsInternal.LayerSelfPart1 + if (useCrossCache) { - layer = layer, - processor = processor - }.Schedule(layer.bucketCount, 1, inputDeps); -#if ENABLE_UNITY_COLLECTIONS_CHECKS - if (disableEntityAliasChecks) - { - jh = new FindPairsInternal.LayerSelfPart2 - { - layer = layer, - processor = processor - }.Schedule(jh); + Physics.WarnCrossCacheUnused(); } - else - { - jh = new FindPairsInternal.LayerSelfPart2_WithSafety - { - layer = layer, - processor = processor - }.ScheduleParallel(2, 1, jh); - } -#else - jh = new FindPairsInternal.LayerSelfPart2 - { - layer = layer, - processor = processor - }.Schedule(jh); -#endif - return jh; + var scheduleMode = disableEntityAliasChecks ? ScheduleMode.ParallelPart1AllowEntityAliasing : ScheduleMode.ParallelPart1; + var jh = new FindPairsInternal.LayerSelfJob(in layer, in processor).ScheduleParallel(inputDeps, scheduleMode); + scheduleMode = disableEntityAliasChecks ? ScheduleMode.ParallelPart2AllowEntityAliasing : ScheduleMode.ParallelPart2; + return new FindPairsInternal.LayerSelfJob(in layer, in processor).ScheduleParallel(jh, scheduleMode); } /// @@ -550,82 +615,17 @@ public JobHandle ScheduleParallel(JobHandle inputDeps = default) /// A JobHandle for the scheduled job public JobHandle ScheduleParallelUnsafe(JobHandle inputDeps = default) { - return new FindPairsInternal.LayerSelfParallelUnsafe - { - layer = layer, - processor = processor - }.ScheduleParallel(2 * layer.bucketCount - 1, 1, inputDeps); - } - #endregion Schedulers - } - - public partial struct FindPairsLayerSelfWithCrossCacheConfig - { - internal T processor; - - internal CollisionLayer layer; - - internal bool disableEntityAliasChecks; - - internal Allocator allocator; - - #region Settings - /// - /// Disables entity aliasing checks on parallel jobs when safety checks are enabled. Use this only when entities can be aliased but body indices must be thread-safe. - /// - public FindPairsLayerSelfWithCrossCacheConfig WithoutEntityAliasingChecks() - { - disableEntityAliasChecks = true; - return this; - } - #endregion - - #region Schedulers - /// - /// Run the FindPairs operation using multiple worker threads in multiple phases. - /// - /// The input dependencies for any layers or processors used in the FindPairs operation - /// The final JobHandle for the scheduled jobs - public JobHandle ScheduleParallel(JobHandle inputDeps = default) - { - var cache = new NativeStream(layer.bucketCount - 1, allocator); - JobHandle jh = new FindPairsInternal.LayerSelfPart1 - { - layer = layer, - processor = processor, - cache = cache.AsWriter() - }.Schedule(2 * layer.bucketCount - 1, 1, inputDeps); -#if ENABLE_UNITY_COLLECTIONS_CHECKS if (disableEntityAliasChecks) { - jh = new FindPairsInternal.LayerSelfPart2 - { - layer = layer, - processor = processor, - cache = cache.AsReader() - }.Schedule(jh); + Physics.WarnEntityAliasingUnchecked(); } - else + if (useCrossCache) { - jh = new FindPairsInternal.LayerSelfPart2_WithSafety - { - layer = layer, - processor = processor, - cache = cache.AsReader() - }.ScheduleParallel(2, 1, jh); + Physics.WarnCrossCacheUnused(); } -#else - jh = new FindPairsInternal.LayerSelfPart2 - { - layer = layer, - processor = processor, - cache = cache.AsReader() - }.Schedule(jh); -#endif - jh = cache.Dispose(jh); - return jh; + return new FindPairsInternal.LayerSelfJob(in layer, in processor).ScheduleParallel(inputDeps, ScheduleMode.ParallelUnsafe); } - #endregion + #endregion Schedulers } public partial struct FindPairsLayerLayerConfig where T : struct, IFindPairsProcessor @@ -636,6 +636,7 @@ public partial struct FindPairsLayerLayerConfig where T : struct, IFindPairsP internal CollisionLayer layerB; internal bool disableEntityAliasChecks; + internal bool useCrossCache; #region Settings /// @@ -648,21 +649,13 @@ public FindPairsLayerLayerConfig WithoutEntityAliasingChecks() } /// - /// Enables usage of a cache for pairs involving the cross bucket. - /// This increases processing time and memory usage, but may decrease latency. - /// This may only be used with ScheduleParallel(). + /// Enables a cross-cache to increase parallelism and reduce latency at the cost of some extra overhead for allocations and cached writing and reading. + /// Currently, this is only supported when using ScheduleParallelByA(). /// - /// The type of allocator to use for the cache - public FindPairsLayerLayerWithCrossCacheConfig WithCrossCache(Allocator cacheAllocator = Allocator.TempJob) + public FindPairsLayerLayerConfig WithCrossCache() { - return new FindPairsLayerLayerWithCrossCacheConfig - { - layerA = layerA, - layerB = layerB, - disableEntityAliasChecks = disableEntityAliasChecks, - processor = processor, - allocator = cacheAllocator - }; + useCrossCache = true; + return this; } #endregion @@ -672,6 +665,14 @@ public FindPairsLayerLayerWithCrossCacheConfig WithCrossCache(Allocator cache /// public void RunImmediate() { + if (disableEntityAliasChecks) + { + Physics.WarnEntityAliasingUnchecked(); + } + if (useCrossCache) + { + Physics.WarnCrossCacheUnused(); + } FindPairsInternal.RunImmediate(in layerA, in layerB, ref processor, false); } @@ -680,12 +681,15 @@ public void RunImmediate() /// public void Run() { - new FindPairsInternal.LayerLayerSingle + if (disableEntityAliasChecks) { - layerA = layerA, - layerB = layerB, - processor = processor - }.Run(); + Physics.WarnEntityAliasingUnchecked(); + } + if (useCrossCache) + { + Physics.WarnCrossCacheUnused(); + } + new FindPairsInternal.LayerLayerJob(in layerA, in layerB, in processor).Run(); } /// @@ -695,97 +699,17 @@ public void Run() /// A JobHandle for the scheduled job public JobHandle ScheduleSingle(JobHandle inputDeps = default) { - return new FindPairsInternal.LayerLayerSingle - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Schedule(inputDeps); - } - - /// - /// Run the FindPairs operation using multiple worker threads in multiple phases. - /// - /// The input dependencies for any layers or processors used in the FindPairs operation - /// The final JobHandle for the scheduled jobs - public JobHandle ScheduleParallel(JobHandle inputDeps = default) - { - JobHandle jh = new FindPairsInternal.LayerLayerPart1 - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Schedule(layerB.bucketCount, 1, inputDeps); -#if ENABLE_UNITY_COLLECTIONS_CHECKS if (disableEntityAliasChecks) { - jh = new FindPairsInternal.LayerLayerPart2 - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Schedule(2, 1, jh); + Physics.WarnEntityAliasingUnchecked(); } - else + if (useCrossCache) { - jh = new FindPairsInternal.LayerLayerPart2_WithSafety - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Schedule(3, 1, jh); + Physics.WarnCrossCacheUnused(); } -#else - jh = new FindPairsInternal.LayerLayerPart2 - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Schedule(2, 1, jh); -#endif - return jh; - } - - /// - /// Run the FindPairs operation using multiple worker threads all at once without entity or body index thread-safety. - /// - /// The input dependencies for any layers or processors used in the FindPairs operation - /// A JobHandle for the scheduled job - public JobHandle ScheduleParallelUnsafe(JobHandle inputDeps = default) - { - return new FindPairsInternal.LayerLayerParallelUnsafe - { - layerA = layerA, - layerB = layerB, - processor = processor - }.ScheduleParallel(3 * layerA.bucketCount - 2, 1, inputDeps); + return new FindPairsInternal.LayerLayerJob(in layerA, in layerB, in processor).ScheduleSingle(inputDeps); } - #endregion Schedulers - } - public partial struct FindPairsLayerLayerWithCrossCacheConfig where T : struct, IFindPairsProcessor - { - internal T processor; - - internal CollisionLayer layerA; - internal CollisionLayer layerB; - - internal bool disableEntityAliasChecks; - - internal Allocator allocator; - - #region Settings - /// - /// Disables entity aliasing checks on parallel jobs when safety checks are enabled. Use this only when entities can be aliased but body indices must be thread-safe. - /// - public FindPairsLayerLayerWithCrossCacheConfig WithoutEntityAliasingChecks() - { - disableEntityAliasChecks = true; - return this; - } - #endregion - - #region Schedulers /// /// Run the FindPairs operation using multiple worker threads in multiple phases. /// @@ -793,142 +717,33 @@ public FindPairsLayerLayerWithCrossCacheConfig WithoutEntityAliasingChecks() /// The final JobHandle for the scheduled jobs public JobHandle ScheduleParallel(JobHandle inputDeps = default) { - var cache = new NativeStream(layerA.bucketCount * 2 - 2, allocator); - - JobHandle jh = new FindPairsInternal.LayerLayerPart1 - { - layerA = layerA, - layerB = layerB, - processor = processor, - cache = cache.AsWriter() - }.Schedule(3 * layerB.bucketCount - 2, 1, inputDeps); -#if ENABLE_UNITY_COLLECTIONS_CHECKS - if (disableEntityAliasChecks) + if (useCrossCache) { - jh = new FindPairsInternal.LayerLayerPart2 - { - layerA = layerA, - layerB = layerB, - processor = processor, - cache = cache.AsReader() - }.Schedule(2, 1, jh); + Physics.WarnCrossCacheUnused(); } - else - { - jh = new FindPairsInternal.LayerLayerPart2_WithSafety - { - layerA = layerA, - layerB = layerB, - processor = processor, - cache = cache.AsReader() - }.Schedule(3, 1, jh); - } -#else - jh = new FindPairsInternal.LayerLayerPart2 - { - layerA = layerA, - layerB = layerB, - processor = processor, - cache = cache.AsReader() - }.Schedule(2, 1, jh); -#endif - jh = cache.Dispose(jh); - return jh; - } - #endregion - } - - internal partial struct FindPairsLayerSelfConfigUnrolled where T : struct, IFindPairsProcessor - { - internal T processor; - - internal CollisionLayer layer; - - internal bool disableEntityAliasChecks; - - #region Settings - /// - /// Disables entity aliasing checks on parallel jobs when safety checks are enabled. Use this only when entities can be aliased but body indices must be thread-safe. - /// - public FindPairsLayerSelfConfigUnrolled WithoutEntityAliasingChecks() - { - disableEntityAliasChecks = true; - return this; - } - #endregion - - #region Schedulers - /// - /// Run the FindPairs operation without using a job. This method can be invoked from inside a job. - /// - public void RunImmediate() - { - FindPairsInternalUnrolled.RunImmediate(layer, processor); - } - - /// - /// Run the FindPairs operation on the main thread using a Bursted job. - /// - public void Run() - { - new FindPairsInternalUnrolled.LayerSelfSingle - { - layer = layer, - processor = processor - }.Run(); + var scheduleMode = ScheduleMode.ParallelPart1; + if (disableEntityAliasChecks) + scheduleMode |= ScheduleMode.AllowEntityAliasing; + var jh = new FindPairsInternal.LayerLayerJob(in layerA, in layerB, in processor).ScheduleParallel(inputDeps, scheduleMode); + scheduleMode = ScheduleMode.ParallelPart2; + if (disableEntityAliasChecks) + scheduleMode |= ScheduleMode.AllowEntityAliasing; + return new FindPairsInternal.LayerLayerJob(in layerA, in layerB, in processor).ScheduleParallel(jh, scheduleMode); } /// - /// Run the FindPairs operation on a single worker thread. + /// Run the FindPairs operation using multiple worker threads in a single phase, without safe write access to the second layer. /// /// The input dependencies for any layers or processors used in the FindPairs operation /// A JobHandle for the scheduled job - public JobHandle ScheduleSingle(JobHandle inputDeps = default) - { - return new FindPairsInternalUnrolled.LayerSelfSingle - { - layer = layer, - processor = processor - }.Schedule(inputDeps); - } - - /// - /// Run the FindPairs operation using multiple worker threads in multiple phases. - /// - /// The input dependencies for any layers or processors used in the FindPairs operation - /// The final JobHandle for the scheduled jobs - public JobHandle ScheduleParallel(JobHandle inputDeps = default) + public JobHandle ScheduleParallelByA(JobHandle inputDeps = default) { - JobHandle jh = new FindPairsInternalUnrolled.LayerSelfPart1 - { - layer = layer, - processor = processor - }.Schedule(layer.bucketCount, 1, inputDeps); -#if ENABLE_UNITY_COLLECTIONS_CHECKS + var scheduleMode = ScheduleMode.ParallelByA; if (disableEntityAliasChecks) - { - jh = new FindPairsInternalUnrolled.LayerSelfPart2 - { - layer = layer, - processor = processor - }.Schedule(jh); - } - else - { - jh = new FindPairsInternalUnrolled.LayerSelfPart2_WithSafety - { - layer = layer, - processor = processor - }.ScheduleParallel(2, 1, jh); - } -#else - jh = new FindPairsInternalUnrolled.LayerSelfPart2 - { - layer = layer, - processor = processor - }.Schedule(jh); -#endif - return jh; + scheduleMode |= ScheduleMode.AllowEntityAliasing; + if (useCrossCache) + scheduleMode |= ScheduleMode.UseCrossCache; + return new FindPairsInternal.LayerLayerJob(in layerA, in layerB, in processor).ScheduleParallel(inputDeps, scheduleMode); } /// @@ -938,128 +753,15 @@ public JobHandle ScheduleParallel(JobHandle inputDeps = default) /// A JobHandle for the scheduled job public JobHandle ScheduleParallelUnsafe(JobHandle inputDeps = default) { - return new FindPairsInternalUnrolled.LayerSelfParallelUnsafe - { - layer = layer, - processor = processor - }.ScheduleParallel(2 * layer.bucketCount - 1, 1, inputDeps); - } - #endregion Schedulers - } - - internal partial struct FindPairsLayerLayerConfigUnrolled where T : struct, IFindPairsProcessor - { - internal T processor; - - internal CollisionLayer layerA; - internal CollisionLayer layerB; - - internal bool disableEntityAliasChecks; - - #region Settings - /// - /// Disables entity aliasing checks on parallel jobs when safety checks are enabled. Use this only when entities can be aliased but body indices must be thread-safe. - /// - public FindPairsLayerLayerConfigUnrolled WithoutEntityAliasingChecks() - { - disableEntityAliasChecks = true; - return this; - } - #endregion - - #region Schedulers - /// - /// Run the FindPairs operation without using a job. This method can be invoked from inside a job. - /// - public void RunImmediate() - { - FindPairsInternalUnrolled.RunImmediate(layerA, layerB, processor); - } - - /// - /// Run the FindPairs operation on the main thread using a Bursted job. - /// - public void Run() - { - new FindPairsInternalUnrolled.LayerLayerSingle - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Run(); - } - - /// - /// Run the FindPairs operation on a single worker thread. - /// - /// The input dependencies for any layers or processors used in the FindPairs operation - /// A JobHandle for the scheduled job - public JobHandle ScheduleSingle(JobHandle inputDeps = default) - { - return new FindPairsInternalUnrolled.LayerLayerSingle - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Schedule(inputDeps); - } - - /// - /// Run the FindPairs operation using multiple worker threads in multiple phases. - /// - /// The input dependencies for any layers or processors used in the FindPairs operation - /// The final JobHandle for the scheduled jobs - public JobHandle ScheduleParallel(JobHandle inputDeps = default) - { - JobHandle jh = new FindPairsInternalUnrolled.LayerLayerPart1 - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Schedule(layerB.bucketCount, 1, inputDeps); -#if ENABLE_UNITY_COLLECTIONS_CHECKS if (disableEntityAliasChecks) { - jh = new FindPairsInternalUnrolled.LayerLayerPart2 - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Schedule(2, 1, jh); + Physics.WarnEntityAliasingUnchecked(); } - else + if (useCrossCache) { - jh = new FindPairsInternalUnrolled.LayerLayerPart2_WithSafety - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Schedule(3, 1, jh); + Physics.WarnCrossCacheUnused(); } -#else - jh = new FindPairsInternalUnrolled.LayerLayerPart2 - { - layerA = layerA, - layerB = layerB, - processor = processor - }.Schedule(2, 1, jh); -#endif - return jh; - } - - /// - /// Run the FindPairs operation using multiple worker threads all at once without entity or body index thread-safety. - /// - /// The input dependencies for any layers or processors used in the FindPairs operation - /// A JobHandle for the scheduled job - public JobHandle ScheduleParallelUnsafe(JobHandle inputDeps = default) - { - return new FindPairsInternalUnrolled.LayerLayerParallelUnsafe - { - layerA = layerA, - layerB = layerB, - processor = processor - }.ScheduleParallel(3 * layerA.bucketCount - 2, 1, inputDeps); + return new FindPairsInternal.LayerLayerJob(in layerA, in layerB, in processor).ScheduleParallel(inputDeps, ScheduleMode.ParallelUnsafe); } #endregion Schedulers } diff --git a/PsyshockPhysics/Physics/Spatial/Queries/Physics.ForEachPair.cs b/PsyshockPhysics/Physics/Spatial/Queries/Physics.ForEachPair.cs new file mode 100644 index 0000000..b54177c --- /dev/null +++ b/PsyshockPhysics/Physics/Spatial/Queries/Physics.ForEachPair.cs @@ -0,0 +1,131 @@ +using Unity.Collections; +using Unity.Entities; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Latios.Psyshock +{ + /// + /// An interface whose Execute method is invoked for each pair found in the PairStream. + /// + public interface IForEachPairProcessor + { + /// + /// The main pair processing callback. Disabled pairs may not receive invocations depending on the settings used. + /// + void Execute(ref PairStream.Pair pair); + + /// + /// An optional callback prior to processing a batch of streams. + /// + /// Returns true if the Execute() and EndBucket() methods should be called. Otherwise, further + /// processing of the bucket is skipped. + bool BeginStreamBatch(ForEachPairBatchContext context) => true; + /// + /// An optional callback following processing a batch of streams. + /// + void EndStreamBatch(ForEachPairBatchContext context) + { + } + } + + /// + /// A context struct passed into BeginStreamBatch and EndStreamBatch of an IForEachPairProcessor which provides + /// additional information about the streams being processed. + /// + public struct ForEachPairBatchContext + { + internal PairStream.Enumerator enumerator; + + /// + /// Gets an enumerator that enumerates over the batch of streams. + /// + public PairStream.Enumerator GetEnumerator() => enumerator; + /// + /// The first stream index in the batch + /// + public int startStreamIndex => enumerator.pair.streamIndex; + /// + /// The number of streams in the batch + /// + public int streamCountInBatch => enumerator.onePastLastStreamIndex - startStreamIndex; + } + + public static partial class Physics + { + public static ForEachPairConfig ForEachPair(in PairStream pairStream, in T processor) where T : struct, IForEachPairProcessor + { + return new ForEachPairConfig + { + processor = processor, + pairStream = pairStream, + includeDisabled = false, + }; + } + } + + public partial struct ForEachPairConfig where T : struct, IForEachPairProcessor + { + internal T processor; + internal PairStream pairStream; + internal bool includeDisabled; + + /// + /// Includes disabled pairs when calling IForEachPairProcessor.Execute() + /// + public ForEachPairConfig IncludeDisabled() + { + includeDisabled = true; + return this; + } + + /// + /// Run the ForEachPair operation without using a job. This method can be invoked from inside a job. + /// + public void RunImmediate() + { + ForEachPairMethods.ExecuteBatch(ref pairStream, ref processor, 0, pairStream.mixedIslandAggregateStream, false, false, includeDisabled); + } + + /// + /// Run the ForEachPair operation on the main thread using a Bursted job. + /// + public void Run() + { + new ForEachPairInternal.ForEachPairJob(in pairStream, in processor, includeDisabled).Run(); + } + + /// + /// Run the ForEachPair operation on a single worker thread. + /// + /// The input dependencies from any previous operation that touches the PairStream + /// A JobHandle for the scheduled job + public JobHandle ScheduleSingle(JobHandle inputDeps) + { + return new ForEachPairInternal.ForEachPairJob(in pairStream, in processor, includeDisabled).ScheduleSingle(inputDeps); + } + + /// + /// Run the ForEachPair operation using multiple worker threads in multiple phases. + /// + /// The input dependencies from any previous operation that touches the PairStream + /// The final JobHandle for the scheduled jobs + public JobHandle ScheduleParallel(JobHandle inputDeps) + { + var jh = new ForEachPairInternal.ForEachPairJob(in pairStream, in processor, includeDisabled).ScheduleParallel(inputDeps, ScheduleMode.ParallelPart1); + ForEachPairMethods.ScheduleBumpVersions(ref pairStream, ref jh); + return new ForEachPairInternal.ForEachPairJob(in pairStream, in processor, includeDisabled).ScheduleParallel(jh, ScheduleMode.ParallelPart2); + } + + /// + /// Run the ForEachPair operation using multiple worker threads all at once without entity thread-safety. + /// + /// The input dependencies from any previous operation that touches the PairStream + /// A JobHandle for the scheduled job + public JobHandle ScheduleParallelUnsafe(JobHandle inputDeps) + { + return new ForEachPairInternal.ForEachPairJob(in pairStream, in processor, includeDisabled).ScheduleParallel(inputDeps, ScheduleMode.ParallelUnsafe); + } + } +} + diff --git a/PsyshockPhysics/Physics/Spatial/Queries/Physics.ForEachPair.cs.meta b/PsyshockPhysics/Physics/Spatial/Queries/Physics.ForEachPair.cs.meta new file mode 100644 index 0000000..1e329e9 --- /dev/null +++ b/PsyshockPhysics/Physics/Spatial/Queries/Physics.ForEachPair.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: af8808effe45ed04481ef1ac79eff5d6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/PsyshockPhysics/Physics/Types/Colliders/CompoundColliderPsyshock.cs b/PsyshockPhysics/Physics/Types/Colliders/CompoundColliderPsyshock.cs index ac3a3b7..c4f50a0 100644 --- a/PsyshockPhysics/Physics/Types/Colliders/CompoundColliderPsyshock.cs +++ b/PsyshockPhysics/Physics/Types/Colliders/CompoundColliderPsyshock.cs @@ -134,7 +134,7 @@ internal void GetScaledStretchedSubCollider(int index, out Collider blobCollider internal struct BlobCollider { #pragma warning disable CS0649 - internal float4x4 storage; + internal float3x4 storage; #pragma warning restore CS0649 } @@ -155,6 +155,11 @@ public struct CompoundColliderBlob /// public Aabb localAabb; + public float3 centerOfMass; + public float3x3 inertiaTensor; + public quaternion unscaledInertiaTensorOrientation; + public float3 unscaledInertiaTensorDiagonal; + /// /// The array of subcolliders /// diff --git a/PsyshockPhysics/Physics/Types/Colliders/ConvexColliderPsyshock.cs b/PsyshockPhysics/Physics/Types/Colliders/ConvexColliderPsyshock.cs index b7254cd..532bf8d 100644 --- a/PsyshockPhysics/Physics/Types/Colliders/ConvexColliderPsyshock.cs +++ b/PsyshockPhysics/Physics/Types/Colliders/ConvexColliderPsyshock.cs @@ -80,6 +80,11 @@ public struct ConvexColliderBlob public Aabb localAabb; + public float3 centerOfMass; + public float3x3 inertiaTensor; + public quaternion unscaledInertiaTensorOrientation; + public float3 unscaledInertiaTensorDiagonal; + public FixedString128Bytes meshName; public struct IndexPair diff --git a/PsyshockPhysics/Physics/Types/CollisionLayer.cs b/PsyshockPhysics/Physics/Types/CollisionLayer.cs index c92b514..881ad05 100644 --- a/PsyshockPhysics/Physics/Types/CollisionLayer.cs +++ b/PsyshockPhysics/Physics/Types/CollisionLayer.cs @@ -52,35 +52,34 @@ public struct CollisionLayerSettings /// public struct CollisionLayer : INativeDisposable { - internal NativeArray bucketStartsAndCounts; - [NativeDisableParallelForRestriction] internal NativeArray xmins; - [NativeDisableParallelForRestriction] internal NativeArray xmaxs; - [NativeDisableParallelForRestriction] internal NativeArray yzminmaxs; - [NativeDisableParallelForRestriction] internal NativeArray intervalTrees; - [NativeDisableParallelForRestriction] internal NativeArray bodies; - [NativeDisableParallelForRestriction] internal NativeArray srcIndices; - internal float3 worldMin; - internal float3 worldAxisStride; - internal int3 worldSubdivisionsPerAxis; - AllocatorManager.AllocatorHandle allocator; - - internal CollisionLayer(int bodyCount, CollisionLayerSettings settings, AllocatorManager.AllocatorHandle allocator) + internal NativeList bucketStartsAndCounts; + internal NativeList xmins; + internal NativeList xmaxs; + internal NativeList yzminmaxs; + internal NativeList intervalTrees; + internal NativeList bodies; + internal NativeList srcIndices; + internal float3 worldMin; + internal float3 worldAxisStride; + internal int3 worldSubdivisionsPerAxis; + internal int bucketCountExcludingNan; + + internal CollisionLayer(CollisionLayerSettings settings, AllocatorManager.AllocatorHandle allocator) { worldMin = settings.worldAabb.min; worldAxisStride = (settings.worldAabb.max - worldMin) / settings.worldSubdivisionsPerAxis; worldSubdivisionsPerAxis = settings.worldSubdivisionsPerAxis; - bucketStartsAndCounts = CollectionHelper.CreateNativeArray( - settings.worldSubdivisionsPerAxis.x * settings.worldSubdivisionsPerAxis.y * settings.worldSubdivisionsPerAxis.z + 2, - allocator, - NativeArrayOptions.UninitializedMemory); - xmins = CollectionHelper.CreateNativeArray(bodyCount, allocator, NativeArrayOptions.UninitializedMemory); - xmaxs = CollectionHelper.CreateNativeArray(bodyCount, allocator, NativeArrayOptions.UninitializedMemory); - yzminmaxs = CollectionHelper.CreateNativeArray(bodyCount, allocator, NativeArrayOptions.UninitializedMemory); - intervalTrees = CollectionHelper.CreateNativeArray(bodyCount, allocator, NativeArrayOptions.UninitializedMemory); - bodies = CollectionHelper.CreateNativeArray(bodyCount, allocator, NativeArrayOptions.UninitializedMemory); - srcIndices = CollectionHelper.CreateNativeArray(bodyCount, allocator, NativeArrayOptions.UninitializedMemory); - this.allocator = allocator; + var buckets = settings.worldSubdivisionsPerAxis.x * settings.worldSubdivisionsPerAxis.y * settings.worldSubdivisionsPerAxis.z + 2; + bucketStartsAndCounts = new NativeList(buckets, allocator); + bucketStartsAndCounts.Resize(buckets, NativeArrayOptions.ClearMemory); + xmins = new NativeList(allocator); + xmaxs = new NativeList(allocator); + yzminmaxs = new NativeList(allocator); + intervalTrees = new NativeList(allocator); + bodies = new NativeList(allocator); + srcIndices = new NativeList(allocator); + bucketCountExcludingNan = buckets - 1; } /// @@ -93,15 +92,15 @@ public CollisionLayer(in CollisionLayer sourceLayer, AllocatorManager.AllocatorH worldMin = sourceLayer.worldMin; worldAxisStride = sourceLayer.worldAxisStride; worldSubdivisionsPerAxis = sourceLayer.worldSubdivisionsPerAxis; + bucketCountExcludingNan = sourceLayer.bucketCountExcludingNan; - bucketStartsAndCounts = CollectionHelper.CreateNativeArray(sourceLayer.bucketStartsAndCounts, allocator); - xmins = CollectionHelper.CreateNativeArray(sourceLayer.xmins, allocator); - xmaxs = CollectionHelper.CreateNativeArray(sourceLayer.xmaxs, allocator); - yzminmaxs = CollectionHelper.CreateNativeArray(sourceLayer.yzminmaxs, allocator); - intervalTrees = CollectionHelper.CreateNativeArray(sourceLayer.intervalTrees, allocator); - bodies = CollectionHelper.CreateNativeArray(sourceLayer.bodies, allocator); - srcIndices = CollectionHelper.CreateNativeArray(sourceLayer.srcIndices, allocator); - this.allocator = allocator; + bucketStartsAndCounts = sourceLayer.bucketStartsAndCounts.Clone(allocator); + xmins = sourceLayer.xmins.Clone(allocator); + xmaxs = sourceLayer.xmaxs.Clone(allocator); + yzminmaxs = sourceLayer.yzminmaxs.Clone(allocator); + intervalTrees = sourceLayer.intervalTrees.Clone(allocator); + bodies = sourceLayer.bodies.Clone(allocator); + srcIndices = sourceLayer.srcIndices.Clone(allocator); } /// @@ -110,10 +109,10 @@ public CollisionLayer(in CollisionLayer sourceLayer, AllocatorManager.AllocatorH /// /// The settings to use for the layer. You typically want to match this with other layers when using FindPairs. /// The Allocator to use for this layer. Despite being empty, this layer is still allocated and may require disposal. - /// A CollisionLayer with zero bodies, but with the bucket distribution matching the specified settings + /// A CollisionLayer with zero bodiesArray, but with the bucket distribution matching the specified settings public static CollisionLayer CreateEmptyCollisionLayer(CollisionLayerSettings settings, AllocatorManager.AllocatorHandle allocator) { - var layer = new CollisionLayer(0, settings, allocator); + var layer = new CollisionLayer(settings, allocator); for (int i = 0; i < layer.bucketStartsAndCounts.Length; i++) layer.bucketStartsAndCounts[i] = 0; layer.worldSubdivisionsPerAxis.x = math.max(1, layer.worldSubdivisionsPerAxis.x); // Ensure IsCreated is true. @@ -126,13 +125,13 @@ public static CollisionLayer CreateEmptyCollisionLayer(CollisionLayerSettings se public void Dispose() { worldSubdivisionsPerAxis = 0; - CollectionHelper.DisposeNativeArray(bucketStartsAndCounts, allocator); - CollectionHelper.DisposeNativeArray(xmins, allocator); - CollectionHelper.DisposeNativeArray(xmaxs, allocator); - CollectionHelper.DisposeNativeArray(yzminmaxs, allocator); - CollectionHelper.DisposeNativeArray(intervalTrees, allocator); - CollectionHelper.DisposeNativeArray(bodies, allocator); - CollectionHelper.DisposeNativeArray(srcIndices, allocator); + bucketStartsAndCounts.Dispose(); + xmins.Dispose(); + xmaxs.Dispose(); + yzminmaxs.Dispose(); + intervalTrees.Dispose(); + bodies.Dispose(); + srcIndices.Dispose(); } /// @@ -143,14 +142,7 @@ public void Dispose() public unsafe JobHandle Dispose(JobHandle inputDeps) { worldSubdivisionsPerAxis = 0; - if (allocator.IsCustomAllocator) - { - // Todo: No DisposeJob for NativeArray from CollectionHelper - inputDeps.Complete(); - Dispose(); - return default; - } - JobHandle* deps = stackalloc JobHandle[7] + JobHandle* deps = stackalloc JobHandle[7] { bucketStartsAndCounts.Dispose(inputDeps), xmins.Dispose(inputDeps), @@ -170,18 +162,18 @@ public unsafe JobHandle Dispose(JobHandle inputDeps) /// /// The number of cells in the layer, including the "catch-all" cell but ignoring the NaN cell /// - public int bucketCount => bucketStartsAndCounts.Length - 1; // For algorithmic purposes, we pretend the nan bucket doesn't exist. + public int bucketCount => bucketCountExcludingNan; // For algorithmic purposes, we pretend the nan bucket doesn't exist. /// /// True if the CollisionLayer has been created /// public bool IsCreated => worldSubdivisionsPerAxis.x > 0; /// - /// Read-Only access to the collider bodies stored in the CollisionLayer ordered by bodyIndex + /// Read-Only access to the collider bodiesArray stored in the CollisionLayer ordered by bodyIndex /// public NativeArray.ReadOnly colliderBodies => bodies.AsReadOnly(); /// /// Read-Only access to the source indices corresponding to each bodyIndex. CollisionLayers - /// reorder bodies for better performance. The source indices specify the original index of + /// reorder bodiesArray for better performance. The source indices specify the original index of /// each body in an EntityQuery or NativeArray of ColliderBody. /// public NativeArray.ReadOnly sourceIndices => srcIndices.AsReadOnly(); @@ -203,16 +195,26 @@ internal BucketSlices GetBucketSlices(int bucketIndex) return new BucketSlices { - xmins = xmins.GetSubArray(start, count), - xmaxs = xmaxs.GetSubArray(start, count), - yzminmaxs = yzminmaxs.GetSubArray(start, count), - intervalTree = intervalTrees.GetSubArray(start, count), - bodies = bodies.GetSubArray(start, count), - srcIndices = srcIndices.GetSubArray(start, count), + xmins = xmins.AsArray().GetSubArray(start, count), + xmaxs = xmaxs.AsArray().GetSubArray(start, count), + yzminmaxs = yzminmaxs.AsArray().GetSubArray(start, count), + intervalTree = intervalTrees.AsArray().GetSubArray(start, count), + bodies = bodies.AsArray().GetSubArray(start, count), + srcIndices = srcIndices.AsArray().GetSubArray(start, count), bucketIndex = bucketIndex, bucketGlobalStart = start }; } + + internal void ResizeUninitialized(int newCount) + { + xmins.ResizeUninitialized(newCount); + xmaxs.ResizeUninitialized(newCount); + yzminmaxs.ResizeUninitialized(newCount); + intervalTrees.ResizeUninitialized(newCount); + bodies.ResizeUninitialized(newCount); + srcIndices.ResizeUninitialized(newCount); + } } internal struct BucketSlices diff --git a/PsyshockPhysics/Physics/Types/PairStream.cs b/PsyshockPhysics/Physics/Types/PairStream.cs new file mode 100644 index 0000000..d749295 --- /dev/null +++ b/PsyshockPhysics/Physics/Types/PairStream.cs @@ -0,0 +1,1147 @@ +using System; +using System.Diagnostics; +using System.Runtime.InteropServices; +using Latios.Unsafe; +using Unity.Burst; +using Unity.Burst.CompilerServices; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Entities; +using Unity.Jobs; +using Unity.Jobs.LowLevel.Unsafe; +using Unity.Mathematics; + +namespace Latios.Psyshock +{ + /// + /// A span of memory which can be stored as a field inside of any object stored within a PairStream. + /// These can be nested. + /// + /// The type of element stored within the span + public unsafe struct StreamSpan where T : unmanaged + { + /// + /// The number of elements in the span + /// + public int length => m_length; + /// + /// Gets the pointer to the raw memory of the span + /// + /// + public T* GetUnsafePtr() => m_ptr; + /// + /// Gets an element of the span by ref + /// + /// The index of the element to fetch + /// The element at the specified index + public ref T this[int index] => ref AsSpan()[index]; + /// + /// Gets an enumerator over the span + /// + /// + public Span.Enumerator GetEnumerator() => AsSpan().GetEnumerator(); + /// + /// Returns the StreamSpan as a .NET Span + /// + /// + public Span AsSpan() => new Span(m_ptr, length); + + internal T* m_ptr; + internal int m_length; + } + + /// + /// A NativeContainer which stores multiple "streams" of pairs and per-pair user-allocated data + /// grouped by the multi-box mechanism of FindPairs. Instances can be concatenated to agregate + /// the products of multiple FindPairs operations. + /// + /// + /// The streams are allocated to allow for full addition of pairs from FindPairs operations + /// (including ParallelUnsafe variants) deterministically. The streams can the be combined + /// using the same allocator, and then be iterated over in parallel using Physics.ForEachPair(). + /// While iterating, pair data can be modified, and new allocations can be performed safely. + /// Pairs that are composed of entities from different buckets in the multi-box can be further + /// parallelized via an islanding algorithm. + /// + [NativeContainer] + public unsafe struct PairStream : INativeDisposable + { + #region Create and Destroy + /// + /// Creates a PairStream using a multi-box with the specified number of cells per axis + /// + /// The number of cells per axis + /// The allocator to use for the PairStream + public PairStream(int3 worldSubdivisionsPerAxis, + AllocatorManager.AllocatorHandle allocator) : this(worldSubdivisionsPerAxis.x * worldSubdivisionsPerAxis.y * worldSubdivisionsPerAxis.z + 1, allocator) + { + } + /// + /// Creates a PairStream using the multi-box from the CollisionLayerSettings + /// + /// The settings that specify the multi-box pairs will conform to + /// The allocator to use for the PairStream + public PairStream(in CollisionLayerSettings settings, AllocatorManager.AllocatorHandle allocator) : this(settings.worldSubdivisionsPerAxis + 1, allocator) + { + } + /// + /// Creates a PairStream using the multi-box from the CollisionLayer. + /// It is safe to pass in a CollisionLayer currently being used in a job. + /// + /// A CollisionLayer with the desired multi-box configuration + /// The allocator to use for the PairStream + public PairStream(in CollisionLayer layerWithSettings, AllocatorManager.AllocatorHandle allocator) : this(layerWithSettings.bucketCount, allocator) + { + } + /// + /// Creates a PairStream using a multi-box that has the specified number of buckets. + /// The cross-bucket is included in this count, but the NaN bucket is excluded. + /// + /// The number of buckets to use. + /// PairStreams will allocate 5n - 1 streams that may be iterated by the enumerator. + /// The allocator to use for the PairStream + public PairStream(int bucketCountExcludingNan, AllocatorManager.AllocatorHandle allocator) + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + CheckAllocator(allocator); + + m_Safety = CollectionHelper.CreateSafetyHandle(allocator); + CollectionHelper.SetStaticSafetyId(ref m_Safety, ref s_staticSafetyId.Data); + AtomicSafetyHandle.SetBumpSecondaryVersionOnScheduleWrite(m_Safety, true); +#endif + + // Parallel Unsafe Bipartite uses 3n - 2 threads where n is the number of cells plus the cross bucket. + // However, 2n - 2 can fall into one of three different streams depending on write-access requirements. + // A pair can fall into the cross bucket, the cell, or a mixed stream. Thus we are at 5n - 2 streams. + // If we add the NaN bucket, we get 5n - 1. And if we reserve one extra slot for islanding, we get 5n. + int totalStreams = 5 * math.max(bucketCountExcludingNan, 1); + data = new SharedContainerData + { + pairHeaders = new UnsafeIndexedBlockList(UnsafeUtility.SizeOf(), 4096 / UnsafeUtility.SizeOf(), totalStreams, allocator), + blockStreamArray = AllocatorManager.Allocate(allocator, totalStreams), + state = AllocatorManager.Allocate(allocator), + expectedBucketCount = bucketCountExcludingNan, + allocator = allocator + }; + + *data.state = default; + + for (int i = 0; i < data.pairHeaders.indexCount; i++) + data.blockStreamArray[i] = default; + } + + /// + /// Disposes the PairStream after the jobs which use it have finished. + /// + /// The JobHandle for any jobs previously using this PairStream + /// The JobHandle for the disposing job scheduled, or inputDeps if no job was scheduled + public JobHandle Dispose(JobHandle inputDeps) + { + var jobHandle = new DisposeJob + { + blockList = data.pairHeaders, + state = data.state, + blockStreams = data.blockStreamArray, + allocator = data.allocator, +#if ENABLE_UNITY_COLLECTIONS_CHECKS + m_Safety = m_Safety +#endif + }.Schedule(inputDeps); + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + AtomicSafetyHandle.Release(m_Safety); +#endif + this = default; + return jobHandle; + } + + /// + /// Disposes the PairStream + /// + public void Dispose() + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + CollectionHelper.DisposeSafetyHandle(ref m_Safety); +#endif + Deallocate(data.state, data.pairHeaders, data.blockStreamArray, data.allocator); + this = default; + } + #endregion + + #region Public API + /// + /// Adds a Pair and allocates memory in the stream for a single instance of type T. Returns a ref to T. + /// The pair will save the reference to T for later lookup. + /// + /// Any unmanaged type that contains data that should be associated with the pair. + /// This type may contain StreamSpan instances. + /// The first entity in the pair + /// The bucket index from the multi-box the first entity belongs to + /// If true, the first entity is given read-write access in a parallel ForEachPair operation + /// The second entity in the pair + /// The bucket index from the multi-box the second entity belongs to + /// If true, the second entity is given read-write access in a parallel ForEachPair operation + /// The pair instance, which can store additional settings and perform additional allocations + /// The reference to the allocated instance of type T + public ref T AddPairAndGetRef(Entity entityA, int bucketA, bool aIsRW, Entity entityB, int bucketB, bool bIsRW, out Pair pair) where T : unmanaged + { + var root = AddPairImpl(entityA, + bucketA, + aIsRW, + entityB, + bucketB, + bIsRW, + UnsafeUtility.SizeOf(), + UnsafeUtility.AlignOf(), + BurstRuntime.GetHashCode32(), + false, + out pair); + pair.header->rootPtr = root; + ref var result = ref UnsafeUtility.AsRef(root); + result = default; + return ref result; + } + + /// + /// Adds a Pair and allocates raw memory in the stream. Returns the pointer to the raw memory. + /// The pair will save the pointer for later lookup. + /// + /// The first entity in the pair + /// The bucket index from the multi-box the first entity belongs to + /// If true, the first entity is given read-write access in a parallel ForEachPair operation + /// The second entity in the pair + /// The bucket index from the multi-box the second entity belongs to + /// If true, the second entity is given read-write access in a parallel ForEachPair operation + /// Specifies the size in bytes to allocate + /// Specifies the required alignment of the allocation + /// The pair instance, which can store additional settings and perform additional allocations + /// The pointer to the allocated data + public void* AddPairRaw(Entity entityA, int bucketA, bool aIsRW, Entity entityB, int bucketB, bool bIsRW, int sizeInBytes, int alignInBytes, out Pair pair) + { + return AddPairImpl(entityA, bucketA, aIsRW, entityB, bucketB, bIsRW, sizeInBytes, alignInBytes, 0, true, out pair); + } + + /// + /// Clone's a pair's entities, buckets, and read-write statuses from another PairStream. + /// All other pair data is reset for the new Pair instance. + /// A new object of type T is allocated for this clone and returned. + /// + /// Any unmanaged type that contains data that should be associated with the pair + /// A pair instance from another stream + /// The newly cloned pair that belongs to this PairStream + /// The reference to the allocated instance of type T + public ref T AddPairFromOtherStreamAndGetRef(in Pair pairFromOtherStream, out Pair pairInThisStream) where T : unmanaged + { + return ref AddPairAndGetRef(pairFromOtherStream.entityA, + pairFromOtherStream.index, + pairFromOtherStream.aIsRW, + pairFromOtherStream.entityB, + pairFromOtherStream.index, + pairFromOtherStream.bIsRW, + out pairInThisStream); + } + + /// + /// Clone's a pair's entities, buckets, and read-write statuses from another PairStream. + /// All other pair data is reset for the new Pair instance. + /// New raw memory is allocated for this clone and returned. + /// + /// A pair instance from another stream + /// The newly cloned pair that belongs to this PairStream + /// The pointer to the allocated data + public void* AddPairFromOtherStreamRaw(in Pair pairFromOtherStream, int sizeInBytes, int alignInBytes, out Pair pairInThisStream) + { + return AddPairRaw(pairFromOtherStream.entityA, + pairFromOtherStream.index, + pairFromOtherStream.aIsRW, + pairFromOtherStream.entityB, + pairFromOtherStream.index, + pairFromOtherStream.bIsRW, + sizeInBytes, + alignInBytes, + out pairInThisStream); + } + + /// + /// Concatenates another PairStream to this PairStream, stealing all allocated memory. + /// Pointers to allocated data associated with pairs (to any level of nesting) are preserved + /// within this PairStream, but will no longer be associated with the old PairStream. + /// This method only works if both this PairStream and the other PairStream have been allocated + /// with the same allocator and use the same multi-box layout. + /// + /// Another PairStream with the same allocator and multi-box configuration, + /// whose pairs and memory should be transfered. After the transfer, the old PairStream is empty of elements + /// but is otherwise in a valid state to collect new pairs using the same multi-box configuration. + public void ConcatenateFrom(ref PairStream pairStreamToStealFrom) + { + CheckWriteAccess(); + pairStreamToStealFrom.CheckWriteAccess(); + CheckStreamsMatch(ref pairStreamToStealFrom); + + data.state->enumeratorVersion++; + data.state->pairPtrVersion++; + pairStreamToStealFrom.data.state->enumeratorVersion++; + pairStreamToStealFrom.data.state->pairPtrVersion++; + + data.pairHeaders.ConcatenateAndStealFromUnordered(ref pairStreamToStealFrom.data.pairHeaders); + for (int i = 0; i < data.pairHeaders.indexCount; i++) + { + ref var stream = ref data.blockStreamArray[i]; + ref var otherStream = ref pairStreamToStealFrom.data.blockStreamArray[i]; + if (!stream.blocks.IsCreated) + { + stream = otherStream; + } + else if (otherStream.blocks.IsCreated) + { + stream.blocks.AddRange(otherStream.blocks); + stream.bytesRemainingInBlock = otherStream.bytesRemainingInBlock; + stream.nextFreeAddress = otherStream.nextFreeAddress; + otherStream.blocks.Clear(); + otherStream.bytesRemainingInBlock = 0; + } + } + } + + /// + /// Gets a ParallelWriter of this PairStream, which can be used inside FindPairs and ForEachPair operations + /// + public ParallelWriter AsParallelWriter() + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + var safety = m_Safety; + CollectionHelper.SetStaticSafetyId(ref safety, ref ParallelWriter.s_staticSafetyId.Data); +#endif + return new ParallelWriter + { + data = data, + threadIndex = -1, +#if ENABLE_UNITY_COLLECTIONS_CHECKS + m_Safety = safety, +#endif + }; + } + + /// + /// Gets an enumerator to enumerate all pairs in the PairStream. Disabled pairs are included. + /// + public Enumerator GetEnumerator() + { + CheckAllocatedAccess(); + return new Enumerator + { + pair = new Pair + { + areEntitiesSafeInContext = false, + data = data, + header = null, + index = 0, + isParallelKeySafe = false, + version = data.state->pairPtrVersion, +#if ENABLE_UNITY_COLLECTIONS_CHECKS + m_Safety = m_Safety, +#endif + }, + currentHeader = null, + enumerator = data.pairHeaders.GetEnumerator(0), + enumeratorVersion = data.state->enumeratorVersion, + onePastLastStreamIndex = mixedIslandAggregateStream, + }; + } + #endregion + + #region Public Types + /// + /// A pair which contains pair metadata, user-assignable metadata, and a pointer + /// to allocated data associated with the pair and owned by the PairStream. + /// + [NativeContainer] + public partial struct Pair + { + /// + /// Allocates multiple contiguous elements of T that will be owned by the PairStream. + /// + /// Any unmanaged type that should be associated with the pair + /// The number of elements to allocate + /// The span of elements allocated + public StreamSpan Allocate(int count, NativeArrayOptions options = NativeArrayOptions.ClearMemory) where T : unmanaged + { + CheckWriteAccess(); + CheckPairPtrVersionMatches(data.state, version); + ref var blocks = ref data.blockStreamArray[index]; + var ptr = blocks.Allocate(count, data.allocator); + var result = new StreamSpan { m_ptr = ptr, m_length = count }; + if (options == NativeArrayOptions.ClearMemory) + result.AsSpan().Clear(); + return result; + } + /// + /// Allocates raw memory that will be owned by the PairStream + /// + /// The number of bytes to allocate + /// The alignment of the allocation + /// A pointer to the raw allocated memory + public void* AllocateRaw(int sizeInBytes, int alignInBytes) + { + CheckWriteAccess(); + CheckPairPtrVersionMatches(data.state, version); + if (sizeInBytes == 0) + return null; + ref var blocks = ref data.blockStreamArray[index]; + return blocks.Allocate(sizeInBytes, alignInBytes, data.allocator); + } + /// + /// Replaces the top-level ref associated with the pair with a new allocation of type T. + /// The old data is still retained but no longer directly referenced by the pair itself. + /// + /// Any unmanaged type that should be associated with the pair. + /// A reference to the allocated instance of type T + public ref T ReplaceRef() where T : unmanaged + { + WriteHeader().flags &= (~PairHeader.kRootPtrIsRaw) & 0xff; + header->rootPtr = AllocateRaw(UnsafeUtility.SizeOf(), UnsafeUtility.AlignOf()); + header->rootTypeHash = BurstRuntime.GetHashCode32(); + ref var result = ref UnsafeUtility.AsRef(header->rootPtr); + result = default; + return ref result; + } + /// + /// Replaces the top-level pointer associated with the pair with a new raw allocation. + /// The old data is still retained but no longer directly referenced by the pair itself. + /// + /// The number of bytes to allocate + /// The alignment of the allocation + /// A pointer to the new allocation + public void* ReplaceRaw(int sizeInBytes, int alignInBytes) + { + WriteHeader().flags |= PairHeader.kRootPtrIsRaw; + header->rootPtr = AllocateRaw(sizeInBytes, alignInBytes); + header->rootTypeHash = 0; + return header->rootPtr; + } + /// + /// Gets a reference to the top-level object associated with the pair. + /// When safety checks are enabled, the type is checked with the type allocated for the pair. + /// + /// The unmanaged type that was allocated for the pair + /// A reference to the data that was allocated for this pair + public ref T GetRef() where T : unmanaged + { + var root = GetRaw(); + CheckTypeHash(); + return ref UnsafeUtility.AsRef(root); + } + /// + /// Gets the raw top-level pointer associated with the pair. If the top-level object + /// was allocated with a specific type, this gets the raw pointer to that object. + /// + /// The raw pointer of the object associated with the pair + public void* GetRaw() => WriteHeader().rootPtr; + + /// + /// A ushort value stored with the pair that may serve any purpose of the user. + /// + public ushort userUShort + { + get => ReadHeader().userUshort; + set => WriteHeader().userUshort = value; + } + /// + /// A byte value stored with the pair that may serve any purpose of the user. + /// A common use for this is to encode an enum specifying the type of object + /// associated with the pair. + /// + public byte userByte + { + get => ReadHeader().userByte; + set => WriteHeader().userByte = value; + } + /// + /// Whether or not the pair is enabled. Disabled pairs may be skipped in a ForEachPair operation. + /// + public bool enabled + { + get => (ReadHeader().flags & PairHeader.kEnabled) == PairHeader.kEnabled; + set => WriteHeader().flags |= PairHeader.kEnabled; + } + + /// + /// If true, the pair's associated object was allocated as a raw pointer. + /// + public bool isRaw => (ReadHeader().flags & PairHeader.kRootPtrIsRaw) == PairHeader.kRootPtrIsRaw; + /// + /// If true, the first entity in the pair was granted read-write access upon creation. + /// However, read-write access may still not be permitted depending on the context + /// (it is disallowed for immediate contexts). + /// + public bool aIsRW => (ReadHeader().flags & PairHeader.kWritableA) == PairHeader.kWritableA; + /// + /// If true, the second entity in the pair was granted read-write access upon creation. + /// However, read-write access may still not be permitted depending on the context + /// (it is disallowed for immediate contexts). + /// + public bool bIsRW => (ReadHeader().flags & PairHeader.kWritableB) == PairHeader.kWritableB; + /// + /// The index of the stream this pair resides in + /// + public int streamIndex + { + get + { + CheckReadAccess(); + CheckPairPtrVersionMatches(data.state, version); + return index; + } + } + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + /// + /// A safe entity handle that can be used inside of PhysicsComponentLookup or PhysicsBufferLookup and corresponds to the + /// owning entity of the first entity in the pair. It can also be implicitly casted and used as a normal entity reference. + /// + public SafeEntity entityA => new SafeEntity + { + m_entity = new Entity + { + Index = math.select(-header->entityA.Index - 1, header->entityA.Index, aIsRW && areEntitiesSafeInContext), + Version = header->entityA.Version + } + }; + /// + /// A safe entity handle that can be used inside of PhysicsComponentLookup or PhysicsBufferLookup and corresponds to the + /// owning entity of the second entity in the pair. It can also be implicitly casted and used as a normal entity reference. + /// + public SafeEntity entityB => new SafeEntity + { + m_entity = new Entity + { + Index = math.select(-header->entityB.Index - 1, header->entityB.Index, bIsRW && areEntitiesSafeInContext), + Version = header->entityB.Version + } + }; +#else + public SafeEntity entityA => new SafeEntity { m_entity = header->entityA }; + public SafeEntity entityB => new SafeEntity { m_entity = header->entityB }; +#endif + } + + /// + /// A key generated via FindPairs that can be used to populate a pair's base data for a ParallelWriter + /// + [NativeContainer] // Similar to FindPairsResult, keep this from escaping the local context + public struct ParallelWriteKey + { + internal Entity entityA; + internal Entity entityB; + internal int streamIndexA; + internal int streamIndexB; + internal int streamIndexCombined; + internal int expectedBucketCount; + } + + [NativeContainer] + [NativeContainerIsAtomicWriteOnly] + public partial struct ParallelWriter + { + /// + /// Adds a Pair from within a FindPairs operation and allocate memory in the stream for a single instance of type T. + /// Returns a ref to T. The pair will save the reference to T for later lookup. + /// + /// Any unmanaged type that contains data that should be associated with the pair. + /// This type may contain StreamSpan instances. + /// A key obtained from a FindPairs operation + /// If true, the first entity is given read-write access in a parallel ForEachPair operation + /// If true, the second entity is given read-write access in a parallel ForEachPair operation + /// The pair instance, which can store additional settings and perform additional allocations + /// The reference to the allocated instance of type T + public ref T AddPairAndGetRef(ParallelWriteKey key, bool aIsRW, bool bIsRW, out Pair pair) where T : unmanaged + { // Todo: Passing the key as an in parameter confuses the compiler. + var root = AddPairImpl(in key, + aIsRW, + bIsRW, + UnsafeUtility.SizeOf(), + UnsafeUtility.AlignOf(), + BurstRuntime.GetHashCode32(), + false, + out pair); + pair.header->rootPtr = root; + ref var result = ref UnsafeUtility.AsRef(root); + result = default; + return ref result; + } + + /// + /// Adds a Pair from within a FindPairs operation and allocate raw memory in the stream. + /// Returns the pointer to the raw memory. The pair will save the pointer for later lookup. + /// + /// A key obtained from a FindPairs operation + /// If true, the first entity is given read-write access in a parallel ForEachPair operation + /// If true, the second entity is given read-write access in a parallel ForEachPair operation + /// Specifies the size in bytes to allocate + /// Specifies the required alignment of the allocation + /// The pair instance, which can store additional settings and perform additional allocations + /// The pointer to the allocated data + public void* AddPairRaw(in ParallelWriteKey key, bool aIsRW, bool bIsRW, int sizeInBytes, int alignInBytes, out Pair pair) + { + return AddPairImpl(in key, aIsRW, bIsRW, sizeInBytes, alignInBytes, 0, true, out pair); + } + + /// + /// Clone's a pair's entities, buckets, and read-write statuses from within a ForEachPair operation on another PairStream. + /// All other pair data is reset for the new Pair instance. + /// A new object of type T is allocated for this clone and returned. + /// + /// Any unmanaged type that contains data that should be associated with the pair + /// A pair instance from another stream + /// The newly cloned pair that belongs to this PairStream + /// The reference to the allocated instance of type T + public ref T AddPairFromOtherStreamAndGetRef(in Pair pairFromOtherStream, out Pair pairInThisStream) where T : unmanaged + { + CheckPairCanBeAddedInParallel(in pairFromOtherStream); + var key = new ParallelWriteKey + { + entityA = pairFromOtherStream.entityA, + entityB = pairFromOtherStream.entityB, + streamIndexA = pairFromOtherStream.index, + streamIndexB = pairFromOtherStream.index, + streamIndexCombined = pairFromOtherStream.index, + expectedBucketCount = pairFromOtherStream.data.expectedBucketCount + }; + return ref AddPairAndGetRef(key, pairFromOtherStream.aIsRW, pairFromOtherStream.bIsRW, out pairInThisStream); + } + /// + /// Clone's a pair's entities, buckets, and read-write statuses from within a ForEachPair operation on another PairStream. + /// All other pair data is reset for the new Pair instance. + /// New raw memory is allocated for this clone and returned. + /// + /// A pair instance from another stream + /// The newly cloned pair that belongs to this PairStream + /// The pointer to the allocated data + public void* AddPairFromOtherStreamRaw(in Pair pairFromOtherStream, int sizeInBytes, int alignInBytes, out Pair pairInThisStream) + { + CheckPairCanBeAddedInParallel(in pairFromOtherStream); + var key = new ParallelWriteKey + { + entityA = pairFromOtherStream.entityA, + entityB = pairFromOtherStream.entityB, + streamIndexA = pairFromOtherStream.index, + streamIndexB = pairFromOtherStream.index, + streamIndexCombined = pairFromOtherStream.index, + expectedBucketCount = pairFromOtherStream.data.expectedBucketCount + }; + return AddPairRaw(in key, pairFromOtherStream.aIsRW, pairFromOtherStream.bIsRW, sizeInBytes, alignInBytes, out pairInThisStream); + } + } + + /// + /// An enumerator over all pairs in the PairStream, or a batch if obtained from a ForEachPair operation. + /// This includes disabled pairs. + /// + public partial struct Enumerator + { + /// + /// The current Pair + /// + public Pair Current + { + get + { + pair.CheckReadAccess(); + CheckSafeToEnumerate(); + CheckValid(); + return pair; + } + } + + /// + /// Advance to the next Pair + /// + /// false if no more pairs are left + public bool MoveNext() + { + pair.CheckReadAccess(); + CheckSafeToEnumerate(); + while (pair.index < onePastLastStreamIndex) + { + if (enumerator.MoveNext()) + { + currentHeader = (PairHeader*)UnsafeUtility.AddressOf(ref enumerator.GetCurrentAsRef()); + pair.header = currentHeader; + return true; + } + pair.index++; + enumerator = pair.data.pairHeaders.GetEnumerator(pair.index); + } + return false; + } + } + #endregion + + #region Public Types Internal Members + partial struct Pair + { + internal SharedContainerData data; + internal int index; + internal int version; + internal PairHeader* header; + internal bool isParallelKeySafe; + internal bool areEntitiesSafeInContext; + + ref PairHeader ReadHeader() + { + CheckReadAccess(); + CheckPairPtrVersionMatches(data.state, version); + return ref *header; + } + + ref PairHeader WriteHeader() + { + CheckWriteAccess(); + CheckPairPtrVersionMatches(data.state, version); + return ref *header; + } + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + //Unfortunately this name is hardcoded into Unity. No idea how EntityCommandBuffer gets away with multiple safety handles. + internal AtomicSafetyHandle m_Safety; +#endif + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckWriteAccess() + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + AtomicSafetyHandle.CheckWriteAndThrow(m_Safety); +#endif + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + internal void CheckReadAccess() + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + AtomicSafetyHandle.CheckReadAndThrow(m_Safety); +#endif + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckTypeHash() where T : unmanaged + { + if ((header->flags & PairHeader.kRootPtrIsRaw) == PairHeader.kRootPtrIsRaw) + throw new InvalidOperationException( + $"Attempted to access a raw allocation using an explicit type. If this is intended, use GetRaw in combination with UnsafeUtility.AsRef."); + if (header->rootTypeHash != BurstRuntime.GetHashCode32()) + throw new InvalidOperationException($"Attempted to access an allocation of a pair using the wrong type."); + } + } + + partial struct ParallelWriter + { + internal SharedContainerData data; + + [NativeSetThreadIndex] + internal int threadIndex; + + bool needsAliasingChecks; + bool needsIslanding; + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + //Unfortunately this name is hardcoded into Unity. No idea how EntityCommandBuffer gets away with multiple safety handles. + internal AtomicSafetyHandle m_Safety; + + internal static readonly SharedStatic s_staticSafetyId = SharedStatic.GetOrCreate(); +#endif + + void* AddPairImpl(in ParallelWriteKey key, bool aIsRW, bool bIsRW, int sizeInBytes, int alignInBytes, int typeHash, bool isRaw, out Pair pair) + { + CheckWriteAccess(); + CheckKeyCompatible(in key); + + // If for some reason the ParallelWriter was created and used in the same thread as an Enumerator, + // We need to bump the version. But we don't want to do this if we are actually running in parallel. + if (threadIndex == -1) + data.state->enumeratorVersion++; + + int targetStream; + if (key.streamIndexA == key.streamIndexB) + targetStream = key.streamIndexA; + else if (!bIsRW) + targetStream = key.streamIndexA; + else if (!aIsRW) + targetStream = key.streamIndexB; + else + targetStream = key.streamIndexCombined; + + // We can safely rely on eventual consistency here as this is a forced value write. + // We only write the first time to avoid dirtying the cache line. + if (!needsIslanding && targetStream == key.streamIndexCombined) + { + needsIslanding = true; + data.state->needsIslanding = true; + } + else if (!needsAliasingChecks && targetStream != key.streamIndexCombined) + { + needsAliasingChecks = true; + data.state->needsAliasChecks = true; + } + + var headerPtr = (PairHeader*)data.pairHeaders.Allocate(targetStream); + * headerPtr = new PairHeader + { + entityA = key.entityA, + entityB = key.entityB, + rootTypeHash = typeHash, + flags = + (byte)((aIsRW ? PairHeader.kWritableA : default) + (bIsRW ? PairHeader.kWritableB : default) + PairHeader.kEnabled + + (isRaw ? PairHeader.kRootPtrIsRaw : default)) + }; + + pair = new Pair + { + data = data, + header = headerPtr, + index = targetStream, + version = data.state->pairPtrVersion, + isParallelKeySafe = true, + areEntitiesSafeInContext = false, +#if ENABLE_UNITY_COLLECTIONS_CHECKS + m_Safety = m_Safety, +#endif + }; + + var root = pair.AllocateRaw(sizeInBytes, alignInBytes); + headerPtr->rootPtr = root; + return root; + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckWriteAccess() + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + AtomicSafetyHandle.CheckWriteAndThrow(m_Safety); +#endif + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckKeyCompatible(in ParallelWriteKey key) + { + if (key.expectedBucketCount != data.expectedBucketCount) + throw new InvalidOperationException( + $"The key is generated from a different base bucket count {key.expectedBucketCount} from what the PairStream was constructed with {data.expectedBucketCount}"); + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckPairCanBeAddedInParallel(in Pair pairFromOtherStream) + { + if (!pairFromOtherStream.isParallelKeySafe) + throw new InvalidOperationException( + $"The pair cannot be safely added to the ParallelWriter because the pair was created from an immediate operation. Add directly to the PairStream instead of the ParallelWriter."); + } + } + + partial struct Enumerator + { + internal Pair pair; + internal UnsafeIndexedBlockList.Enumerator enumerator; + internal PairHeader* currentHeader; + internal int onePastLastStreamIndex; + internal int enumeratorVersion; + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckSafeToEnumerate() + { + if (pair.data.state->enumeratorVersion != enumeratorVersion) + throw new InvalidOperationException($"The PairStream Enumerator has been invalidated."); + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckValid() + { + if (pair.header == null) + throw new InvalidOperationException("Attempted to read the Current value when there is none, either because MoveNext() has not been called or returned false."); + if (pair.header != currentHeader) + throw new InvalidOperationException( + "The Pair value in the enumerator was overwritten. Do not directly assign a different Pair instance to the ref passed into the processor."); + } + } + #endregion + + #region Internal Types + [StructLayout(LayoutKind.Sequential, Size = 32)] // Force to 8-byte alignment + internal struct PairHeader + { + public Entity entityA; + public Entity entityB; + public void* rootPtr; + public int rootTypeHash; + public ushort userUshort; + public byte userByte; + public byte flags; + + public const byte kWritableA = 0x1; + public const byte kWritableB = 0x2; + public const byte kEnabled = 0x4; + public const byte kRootPtrIsRaw = 0x8; + } + + internal struct BlockPtr + { + public byte* ptr; + public int byteCount; + } + + [StructLayout(LayoutKind.Sequential, Size = JobsUtility.CacheLineSize)] + internal struct BlockStream + { + public UnsafeList blocks; + public byte* nextFreeAddress; + public int bytesRemainingInBlock; + + public T* Allocate(int count, AllocatorManager.AllocatorHandle allocator) where T : unmanaged + { + var neededBytes = UnsafeUtility.SizeOf() * count; + return (T*)Allocate(neededBytes, UnsafeUtility.AlignOf(), allocator); + } + + public void* Allocate(int sizeInBytes, int alignInBytes, AllocatorManager.AllocatorHandle allocator) + { + var neededBytes = sizeInBytes; + if (Hint.Unlikely(!CollectionHelper.IsAligned(nextFreeAddress, alignInBytes))) + { + var newAddress = (byte*)CollectionHelper.Align((ulong)nextFreeAddress, (ulong)alignInBytes); + var diff = newAddress - nextFreeAddress; + bytesRemainingInBlock -= (int)diff; + } + + if (Hint.Unlikely(neededBytes > bytesRemainingInBlock)) + { + if (Hint.Unlikely(!blocks.IsCreated)) + { + blocks = new UnsafeList(8, allocator); + } + var blockSize = math.max(neededBytes, 16 * 1024); + var newBlock = new BlockPtr + { + byteCount = blockSize, + ptr = AllocatorManager.Allocate(allocator, blockSize) + }; + UnityEngine.Debug.Assert(CollectionHelper.IsAligned(newBlock.ptr, alignInBytes)); + blocks.Add(newBlock); + nextFreeAddress = newBlock.ptr; + bytesRemainingInBlock = neededBytes; + } + + var result = nextFreeAddress; + bytesRemainingInBlock -= neededBytes; + nextFreeAddress += neededBytes; + return result; + } + } + + internal struct State + { + public int enumeratorVersion; + public int pairPtrVersion; + public bool needsAliasChecks; + public bool needsIslanding; + } + + internal struct SharedContainerData + { + public UnsafeIndexedBlockList pairHeaders; + + [NativeDisableUnsafePtrRestriction] + public BlockStream* blockStreamArray; + + [NativeDisableUnsafePtrRestriction] + public State* state; + + public int expectedBucketCount; + public AllocatorManager.AllocatorHandle allocator; + } + #endregion + + #region Internal Structure + internal SharedContainerData data; + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + //Unfortunately this name is hardcoded into Unity. No idea how EntityCommandBuffer gets away with multiple safety handles. + internal AtomicSafetyHandle m_Safety; + + static readonly SharedStatic s_staticSafetyId = SharedStatic.GetOrCreate(); +#endif + #endregion + + #region Internal Helpers + internal int firstMixedBucketStream => 3 * data.expectedBucketCount; + internal int nanBucketStream => 5 * data.expectedBucketCount - 2; + internal int mixedIslandAggregateStream => 5 * data.expectedBucketCount - 1; + + void* AddPairImpl(Entity entityA, + int bucketA, + bool aIsRW, + Entity entityB, + int bucketB, + bool bIsRW, + int sizeInBytes, + int alignInBytes, + int typeHash, + bool isRaw, + out Pair pair) + { + CheckWriteAccess(); + CheckTargetBucketIsValid(bucketA); + CheckTargetBucketIsValid(bucketB); + + data.state->enumeratorVersion++; + + int targetStream; + if (bucketA == bucketB) + targetStream = bucketA; + else if (!bIsRW) + targetStream = bucketA; + else if (!aIsRW) + targetStream = bucketB; + else + targetStream = firstMixedBucketStream; + + if (targetStream == firstMixedBucketStream) + data.state->needsIslanding = true; + else + { + data.state->needsAliasChecks = true; + if (targetStream == data.expectedBucketCount) + targetStream = nanBucketStream; + else + targetStream *= 3; + } + + var headerPtr = (PairHeader*)data.pairHeaders.Allocate(targetStream); + *headerPtr = new PairHeader + { + entityA = entityA, + entityB = entityB, + rootTypeHash = typeHash, + flags = + (byte)((aIsRW ? PairHeader.kWritableA : default) + (bIsRW ? PairHeader.kWritableB : default) + PairHeader.kEnabled + + (isRaw ? PairHeader.kRootPtrIsRaw : default)) + }; + + pair = new Pair + { + data = data, + header = headerPtr, + index = targetStream, + version = data.state->pairPtrVersion, + isParallelKeySafe = false, + areEntitiesSafeInContext = false, +#if ENABLE_UNITY_COLLECTIONS_CHECKS + m_Safety = m_Safety, +#endif + }; + + var root = pair.AllocateRaw(sizeInBytes, alignInBytes); + headerPtr->rootPtr = root; + return root; + } + + private static void Deallocate(State* state, UnsafeIndexedBlockList blockList, BlockStream* blockStreams, AllocatorManager.AllocatorHandle allocator) + { + for (int i = 0; i < blockList.indexCount; i++) + { + var blockStream = blockStreams[i]; + if (blockStream.blocks.IsCreated) + { + foreach (var block in blockStream.blocks) + AllocatorManager.Free(allocator, block.ptr, block.byteCount); + blockStream.blocks.Dispose(); + } + } + + AllocatorManager.Free(allocator, blockStreams, blockList.indexCount); + AllocatorManager.Free(allocator, state, 1); + blockList.Dispose(); + } + + [BurstCompile] + private struct DisposeJob : IJob + { + [NativeDisableUnsafePtrRestriction] + public State* state; + + public UnsafeIndexedBlockList blockList; + + [NativeDisableUnsafePtrRestriction] + public BlockStream* blockStreams; + + public AllocatorManager.AllocatorHandle allocator; + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + internal AtomicSafetyHandle m_Safety; +#endif + + public void Execute() + { + Deallocate(state, blockList, blockStreams, allocator); + } + } + #endregion + + #region Safety Checks + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + internal void CheckWriteAccess() + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + AtomicSafetyHandle.CheckWriteAndThrow(m_Safety); +#endif + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckAllocatedAccess() + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + AtomicSafetyHandle.CheckExistsAndThrow(m_Safety); +#endif + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + static void CheckAllocator(AllocatorManager.AllocatorHandle allocator) + { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (allocator.ToAllocator <= Allocator.None) + throw new System.InvalidOperationException("Allocator cannot be Invalid or None"); +#endif + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckTargetBucketIsValid(int bucket) + { + if (bucket < 0 || bucket > data.expectedBucketCount) // greater than because add 1 for NaN bucket + throw new ArgumentOutOfRangeException($"The target bucket {bucket} is out of range of max buckets {data.expectedBucketCount}"); + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + void CheckStreamsMatch(ref PairStream other) + { + if (data.expectedBucketCount != other.data.expectedBucketCount) + throw new InvalidOperationException($"The streams do not have matching bucket counts: {data.expectedBucketCount} vs {other.data.expectedBucketCount}."); + if (data.allocator != other.data.allocator) + throw new InvalidOperationException($"The allocators are not the same. Memory stealing cannot be safely performed."); + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + static void CheckPairPtrVersionMatches(State* state, int version) + { + if (state->pairPtrVersion != version) + throw new InvalidOperationException($"The pair allocator has been invalidated by a concatenate operation."); + } + + [Conditional("ENABLE_UNITY_COLLECTIONS_CHECKS")] + static void CheckEnumerationVersionMatches(State* state, int version) + { + if (state->pairPtrVersion != version) + throw new InvalidOperationException($"The enumerator has been invalidated by an addition or concatenate operation."); + } + + #endregion + } +} + diff --git a/PsyshockPhysics/Physics/Types/PairStream.cs.meta b/PsyshockPhysics/Physics/Types/PairStream.cs.meta new file mode 100644 index 0000000..78ea883 --- /dev/null +++ b/PsyshockPhysics/Physics/Types/PairStream.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ec1be81c93fe10c4099ba579b0178eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/PsyshockPhysics/Physics/Types/PhysicsComponentLookup.cs b/PsyshockPhysics/Physics/Types/PhysicsComponentLookup.cs index c3788c6..069a09f 100644 --- a/PsyshockPhysics/Physics/Types/PhysicsComponentLookup.cs +++ b/PsyshockPhysics/Physics/Types/PhysicsComponentLookup.cs @@ -18,12 +18,14 @@ namespace Latios.Psyshock [NativeContainer] public struct SafeEntity { - internal Entity entity; + internal Entity m_entity; + + public Entity entity => (Entity)this; #if ENABLE_UNITY_COLLECTIONS_CHECKS public static implicit operator Entity(SafeEntity e) => new Entity { - Index = math.select(e.entity.Index, math.abs(e.entity.Index + 1), e.entity.Index < 0), Version = e.entity.Version + Index = math.select(e.m_entity.Index, math.abs(e.m_entity.Index + 1), e.m_entity.Index < 0), Version = e.m_entity.Version }; #else public static implicit operator Entity(SafeEntity e) => e.entity; @@ -52,12 +54,12 @@ public T this[SafeEntity safeEntity] get { ValidateSafeEntityIsSafe(safeEntity); - return lookup[safeEntity.entity]; + return lookup[safeEntity.m_entity]; } set { ValidateSafeEntityIsSafe(safeEntity); - lookup[safeEntity.entity] = value; + lookup[safeEntity.m_entity] = value; } } @@ -149,7 +151,7 @@ public void Update(SystemBase system) static void ValidateSafeEntityIsSafe(SafeEntity safeEntity) { #if ENABLE_UNITY_COLLECTIONS_CHECKS - if (safeEntity.entity.Index < 0) + if (safeEntity.m_entity.Index < 0) { throw new InvalidOperationException("PhysicsComponentDataFromEntity cannot be used inside a RunImmediate context. Use ComponentDataFromEntity instead."); } @@ -180,7 +182,7 @@ public DynamicBuffer this[SafeEntity safeEntity] get { ValidateSafeEntityIsSafe(safeEntity); - return lookup[safeEntity.entity]; + return lookup[safeEntity.m_entity]; } } @@ -203,7 +205,7 @@ public bool TryGetComponent(SafeEntity safeEntity, out DynamicBuffer bufferDa /// This check is always valid regardless of whether such a buffer would be /// safe to access. /// - public bool HasBuffer(SafeEntity safeEntity) => lookup.HasBuffer(safeEntity.entity); + public bool HasBuffer(SafeEntity safeEntity) => lookup.HasBuffer(safeEntity.m_entity); /// /// This is identical to BufferFromEntity.DidChange(). @@ -259,7 +261,7 @@ public void Update(SystemBase system) static void ValidateSafeEntityIsSafe(SafeEntity safeEntity) { #if ENABLE_UNITY_COLLECTIONS_CHECKS - if (safeEntity.entity.Index < 0) + if (safeEntity.m_entity.Index < 0) { throw new InvalidOperationException("PhysicsBufferFromEntity cannot be used inside a RunImmediate context. Use BufferFromEntity instead."); } @@ -322,7 +324,7 @@ public TransformAspect this[SafeEntity entity] static void ValidateSafeEntityIsSafe(SafeEntity safeEntity) { #if ENABLE_UNITY_COLLECTIONS_CHECKS - if (safeEntity.entity.Index < 0) + if (safeEntity.m_entity.Index < 0) { throw new InvalidOperationException("PhysicsBufferFromEntity cannot be used inside a RunImmediate context. Use BufferFromEntity instead."); } diff --git a/PsyshockPhysics/Physics/Types/QueryResults.cs b/PsyshockPhysics/Physics/Types/QueryResults.cs index 7687840..6df37db 100644 --- a/PsyshockPhysics/Physics/Types/QueryResults.cs +++ b/PsyshockPhysics/Physics/Types/QueryResults.cs @@ -1,4 +1,6 @@ using Latios.Transforms; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; using Unity.Entities; using Unity.Mathematics; @@ -181,5 +183,29 @@ public struct LayerBodyInfo /// public TransformQvvs transform => body.transform; } + + /// + /// A collector you can use in a call to Physics.DistanceBetweenAll() to gather all + /// results in a list and then iterate over them in a foreach statement. + /// + public struct DistanceBetweenAllCache : IDistanceBetweenAllProcessor + { + UnsafeList results; + + public void Begin(in DistanceBetweenAllContext context) => results.Length = 0; + + public void Execute(in ColliderDistanceResult result) + { + if (!results.IsCreated) + results = new UnsafeList(8, Allocator.Temp); + results.Add(result); + } + + public int length => results.Length; + + public ColliderDistanceResult this[int index] => results[index]; + + public UnsafeList.Enumerator GetEnumerator() => results.GetEnumerator(); + } } diff --git a/README.md b/README.md index 73bc1dd..c502fd0 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,17 @@ ![](https://github.com/Dreaming381/Latios-Framework-Documentation/blob/554a583e217bfe5bf38ece0ed65b22c33711afc6/media/bf2cb606139bb3ca01fe1c4c9f92cdf7.png) -# Latios Framework for Unity ECS – [0.9.4] +# Latios Framework for Unity ECS – [0.10.0-alpha.1] + +**This is a prerelease version of the Latios Framework version 0.10 which is +still under development. Changelogs and Documentation, including the remainder +of this README, have not been updated to reflect the new features and changes in +0.10. Git hashes may not be preserved on transition to beta or official +release.** + +**You are still welcome to submit bug reports and PRs for this and future +prerelease versions!** + +**This version of the alpha uses Unity 2022.3.13 with Entities 1.1.0-pre.3.** The Latios Framework is a powerful suite of high-performance low-level APIs and feature-sets for Unity’s ECS which aims to give you back control over your diff --git a/Transforms/CachedQvvs/Authoring/TransformBakeUtils.cs b/Transforms/CachedQvvs/Authoring/TransformBakeUtils.cs index 8ef0786..3cbdf36 100644 --- a/Transforms/CachedQvvs/Authoring/TransformBakeUtils.cs +++ b/Transforms/CachedQvvs/Authoring/TransformBakeUtils.cs @@ -29,13 +29,13 @@ public static TransformQvvs GetQvvsRelativeTo(this Transform current, Transform var original = current; GetScaleAndStretch(current.localScale, out var scale, out var stretch); - var currentQvvs = new TransformQvvs(current.position, current.rotation, scale, stretch); + var currentQvvs = new TransformQvvs(current.localPosition, current.localRotation, scale, stretch); - while (current != targetSpace && current.parent != null) + while (current.parent != null && current.parent != targetSpace) { current = current.parent; GetScaleAndStretch(current.localScale, out scale, out stretch); - var parentQvvs = new TransformQvvs(current.position, current.rotation, scale, stretch); + var parentQvvs = new TransformQvvs(current.localPosition, current.localRotation, scale, stretch); currentQvvs = qvvs.mul(in parentQvvs, in currentQvvs); } @@ -43,13 +43,13 @@ public static TransformQvvs GetQvvsRelativeTo(this Transform current, Transform return currentQvvs; GetScaleAndStretch(targetSpace.localScale, out scale, out stretch); - var targetToWorldQvvs = new TransformQvvs(targetSpace.position, targetSpace.rotation, scale, stretch); + var targetToWorldQvvs = new TransformQvvs(targetSpace.localPosition, targetSpace.localRotation, scale, stretch); - while (targetSpace != original && targetSpace.parent != null) + while (targetSpace.parent != null && targetSpace != original) { targetSpace = targetSpace.parent; GetScaleAndStretch(targetSpace.localScale, out scale, out stretch); - var parentQvvs = new TransformQvvs(targetSpace.position, targetSpace.rotation, scale, stretch); + var parentQvvs = new TransformQvvs(targetSpace.localPosition, targetSpace.localRotation, scale, stretch); targetToWorldQvvs = qvvs.mul(in parentQvvs, in targetToWorldQvvs); } diff --git a/Transforms/UnityTransforms/TransformSuperSystems.cs b/Transforms/UnityTransforms/TransformSuperSystems.cs index 73d3a02..8a24a59 100644 --- a/Transforms/UnityTransforms/TransformSuperSystems.cs +++ b/Transforms/UnityTransforms/TransformSuperSystems.cs @@ -19,6 +19,18 @@ protected override void CreateSystems() EnableSystemSorting = true; } } + + [DisableAutoCreation] + [UpdateInGroup(typeof(LatiosWorldSyncGroup))] + public partial struct GameObjectEntityBindingSystem : ISystem {} + + [DisableAutoCreation] + [UpdateInGroup(typeof(TransformSystemGroup), OrderFirst = true)] + public partial struct CopyGameObjectTransformToEntitySystem : ISystem {} + + [DisableAutoCreation] + [UpdateInGroup(typeof(TransformSystemGroup), OrderLast = true)] + public partial struct CopyGameObjectTransformFromEntitySystem : ISystem {} } #endif diff --git a/Transforms/UnityTransforms/TransformsBootstrap.cs b/Transforms/UnityTransforms/TransformsBootstrap.cs new file mode 100644 index 0000000..a2d2e8c --- /dev/null +++ b/Transforms/UnityTransforms/TransformsBootstrap.cs @@ -0,0 +1,20 @@ +#if !LATIOS_TRANSFORMS_UNCACHED_QVVS && LATIOS_TRANSFORMS_UNITY + +using Unity.Collections; +using Unity.Entities; +using Unity.Mathematics; + +namespace Latios.Transforms +{ + public static class TransformsBootstrap + { + public static void InstallGameObjectEntitySynchronization(LatiosWorld world, ComponentSystemGroup defaultComponentSystemGroup, bool extreme = false) + { + BootstrapTools.InjectSystem(TypeManager.GetSystemTypeIndex(), world); + BootstrapTools.InjectSystem(TypeManager.GetSystemTypeIndex(), world); + BootstrapTools.InjectSystem(TypeManager.GetSystemTypeIndex(), world); + } + } +} +#endif + diff --git a/Transforms/UnityTransforms/TransformsBootstrap.cs.meta b/Transforms/UnityTransforms/TransformsBootstrap.cs.meta new file mode 100644 index 0000000..49bd009 --- /dev/null +++ b/Transforms/UnityTransforms/TransformsBootstrap.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1040c8752bf364b43a11e95cbcd62f73 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/package.json b/package.json index 6fb2eb8..69d4158 100644 --- a/package.json +++ b/package.json @@ -1,7 +1,7 @@ { "name": "com.latios.latiosframework", "displayName": "Latios Framework for ECS", - "version": "0.9.4", + "version": "0.10.0-alpha.1", "unity": "2022.3", "description": "Latios Framework for ECS is a collection of tools, algorithms, and API extensions developed by a hardcore hobbyist game developer.\n\nThis package includes all of the following modules:\n\u25aa Core\n\u25aa QVVS Transforms\n\u25aa Psyshock Physics\n\u25aa Myri Audio\n\u25aa Kinemation Animation and Rendering\nu25aa Caligraphics\nu25aa Mimic\n\nExamples: \n\u25aa Latios Space Shooter Sample - https://github.com/Dreaming381/lsss-wip \n\u25aa Mini Demos - https://github.com/Dreaming381/LatiosFrameworkMiniDemos \n\u25aa Free Parking - https://github.com/Dreaming381/Free-Parking", "documentationURL": "https://github.com/Dreaming381/Latios-Framework-Documentation",