├── README.md ├── Runtime.meta ├── Authoring.meta ├── Runtime ├── Systems.meta ├── Components.meta ├── Components │ ├── Velocity.cs.meta │ └── Velocity.cs ├── NZCore.Pathfinding.asmdef.meta ├── Systems │ ├── PathfinderSystem.cs.meta │ ├── FindPathSystem.cs.meta │ ├── WalkPathSystem.cs.meta │ ├── FindPathSystem.cs │ ├── WalkPathSystem.cs │ └── PathfinderSystem.cs └── NZCore.Pathfinding.asmdef ├── Authoring ├── NZCore.Pathfinding.asmdef.meta ├── Velocity_Authoring.cs.meta ├── NZCore.Pathfinding.asmdef └── Velocity_Authoring.cs └── LICENSE /README.md: -------------------------------------------------------------------------------- 1 | # NZCore.Pathfinding 2 | Pathfinding - using Unity Navmesh, ECS and Burst 3 | -------------------------------------------------------------------------------- /Runtime.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 8b571788c9d34c90a31260bfdec3443d 3 | timeCreated: 1682968331 -------------------------------------------------------------------------------- /Authoring.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: bb6ec9cb415449c889c75634b1df71f1 3 | timeCreated: 1684608826 -------------------------------------------------------------------------------- /Runtime/Systems.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 85a503a98eaa4fc9a90c857aa6974a6e 3 | timeCreated: 1684608923 -------------------------------------------------------------------------------- /Runtime/Components.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 388816823681489f9d9a8fcdc677e3e6 3 | timeCreated: 1684608639 -------------------------------------------------------------------------------- /Runtime/Components/Velocity.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: bc6a4ce995874626be9456c950b3b810 3 | timeCreated: 1664839151 -------------------------------------------------------------------------------- /Authoring/NZCore.Pathfinding.asmdef.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 1f898bc539c94697b333590ece51eea9 3 | timeCreated: 1684608837 -------------------------------------------------------------------------------- /Runtime/NZCore.Pathfinding.asmdef.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 2485d09016534aa19b93d80a111a365d 3 | timeCreated: 1682968319 -------------------------------------------------------------------------------- /Runtime/Systems/PathfinderSystem.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 52ed7f158b104eeb8d7095890be7c232 3 | timeCreated: 1682968342 -------------------------------------------------------------------------------- /Runtime/Components/Velocity.cs: -------------------------------------------------------------------------------- 1 | using Unity.Entities; 2 | using Unity.Mathematics; 3 | 4 | namespace NZCore.Pathfinding 5 | { 6 | public struct Velocity : IComponentData 7 | { 8 | public float3 Value; 9 | } 10 | } -------------------------------------------------------------------------------- /Authoring/Velocity_Authoring.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 941e7e95bff478e4a9bfff4ddd91fb76 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Runtime/Systems/FindPathSystem.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 287c2336d04e2fa46b21e01a2d7f8c41 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Runtime/Systems/WalkPathSystem.cs.meta: -------------------------------------------------------------------------------- 1 | fileFormatVersion: 2 2 | guid: 547a5980af219514c89b5d6b6d1b4cd4 3 | MonoImporter: 4 | externalObjects: {} 5 | serializedVersion: 2 6 | defaultReferences: [] 7 | executionOrder: 0 8 | icon: {instanceID: 0} 9 | userData: 10 | assetBundleName: 11 | assetBundleVariant: 12 | -------------------------------------------------------------------------------- /Authoring/NZCore.Pathfinding.asmdef: -------------------------------------------------------------------------------- 1 | { 2 | "name": "NZCore.Pathfinding.Authoring", 3 | "rootNamespace": "", 4 | "references": [ 5 | "Unity.Entities", 6 | "Unity.Entities.Hybrid", 7 | "Unity.Mathematics", 8 | "NZCore.Pathfinding", 9 | "Unity.Collections" 10 | ], 11 | "includePlatforms": [], 12 | "excludePlatforms": [], 13 | "allowUnsafeCode": true, 14 | "overrideReferences": false, 15 | "precompiledReferences": [], 16 | "autoReferenced": true, 17 | "defineConstraints": [], 18 | "versionDefines": [], 19 | "noEngineReferences": false 20 | } -------------------------------------------------------------------------------- /Runtime/NZCore.Pathfinding.asmdef: -------------------------------------------------------------------------------- 1 | { 2 | "name": "NZCore.Pathfinding", 3 | "rootNamespace": "", 4 | "references": [ 5 | "Unity.Entities", 6 | "Unity.Entities.Hybrid", 7 | "Unity.Burst", 8 | "Unity.Collections", 9 | "Unity.AI.Navigation", 10 | "Unity.Mathematics", 11 | "Unity.Transforms", 12 | "Unity.Physics.Hybrid", 13 | "NZCore" 14 | ], 15 | "includePlatforms": [], 16 | "excludePlatforms": [], 17 | "allowUnsafeCode": true, 18 | "overrideReferences": false, 19 | "precompiledReferences": [], 20 | "autoReferenced": true, 21 | "defineConstraints": [], 22 | "versionDefines": [], 23 | "noEngineReferences": false 24 | } -------------------------------------------------------------------------------- /Authoring/Velocity_Authoring.cs: -------------------------------------------------------------------------------- 1 | using Unity.Entities; 2 | using Unity.Mathematics; 3 | using UnityEngine; 4 | 5 | namespace NZCore.Pathfinding.Authoring 6 | { 7 | public class Velocity_Authoring : MonoBehaviour 8 | { 9 | public float3 velocity; 10 | 11 | public class Velocity_Authoring_Baker : Baker 12 | { 13 | public override void Bake(Velocity_Authoring authoring) 14 | { 15 | var entity = GetEntity(TransformUsageFlags.Dynamic); 16 | 17 | AddComponent(entity, new Velocity() 18 | { 19 | Value = authoring.velocity 20 | }); 21 | } 22 | } 23 | } 24 | } -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 enzi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Runtime/Systems/FindPathSystem.cs: -------------------------------------------------------------------------------- 1 | using System.Resources; 2 | using Unity.Burst; 3 | using Unity.Collections; 4 | using Unity.Entities; 5 | using Unity.Burst.Intrinsics; 6 | using Unity.Entities.Content; 7 | using Unity.Mathematics; 8 | using Unity.Transforms; 9 | using UnityEngine; 10 | using UnityEngine.Experimental.AI; 11 | 12 | namespace NZCore.Pathfinding 13 | { 14 | public struct FindPath : IComponentData 15 | { 16 | public Entity source; 17 | public Entity target; 18 | 19 | public float3 targetPosition; 20 | 21 | public float speed; 22 | public float requiredMinDistanceSq; 23 | 24 | public int pathId; 25 | public PathStatus PathStatus; 26 | public int pathWalkerIndex; 27 | } 28 | 29 | [InternalBufferCapacity(0)] 30 | public struct PathBuffer : IBufferElementData 31 | { 32 | public float3 position; 33 | } 34 | 35 | [CreateAfter(typeof(PathfinderSystem))] 36 | [UpdateInGroup(typeof(Stage1SystemGroup))] 37 | public unsafe partial struct FindPathSystem : ISystem 38 | { 39 | private EntityQuery query; 40 | 41 | private NavMeshWorld _navMeshWorld; 42 | private NavMeshQuery _navMeshQuery; 43 | 44 | NativeArray m_Costs; 45 | 46 | [BurstCompile] 47 | public void OnCreate(ref SystemState state) 48 | { 49 | query = new EntityQueryBuilder(Allocator.Temp) 50 | .WithAll() 51 | .Build(ref state); 52 | 53 | _navMeshWorld = NavMeshWorld.GetDefaultWorld(); 54 | _navMeshQuery = new NavMeshQuery(_navMeshWorld, Allocator.Persistent, 0); 55 | 56 | m_Costs = new NativeArray(32, Allocator.Persistent); 57 | for (var i = 0; i < m_Costs.Length; ++i) 58 | m_Costs[i] = 1.0f; 59 | 60 | SystemAPI.GetSingletonRW(); 61 | } 62 | 63 | [BurstCompile] 64 | public void OnDestroy(ref SystemState state) 65 | { 66 | _navMeshQuery.Dispose(); 67 | m_Costs.Dispose(); 68 | } 69 | 70 | [BurstCompile] 71 | public void OnUpdate(ref SystemState state) 72 | { 73 | var commandBuffer = SystemAPI.GetSingleton().CreateCommandBuffer(state.WorldUnmanaged); 74 | var pathRequests = SystemAPI.GetSingleton(); 75 | 76 | pathRequests.Requests.Update(); 77 | 78 | state.Dependency = new FindPathJob() 79 | { 80 | CommandBuffer = commandBuffer.AsParallelWriter(), 81 | Entities_ReadHandle = SystemAPI.GetEntityTypeHandle(), 82 | PathRequests = pathRequests.Requests.AsParallelWriter(), 83 | 84 | LocalTransform_Lookup = SystemAPI.GetComponentLookup(true), 85 | NavMeshQuery = _navMeshQuery, 86 | FindPath_WriteHandle = SystemAPI.GetComponentTypeHandle(false) 87 | }.ScheduleParallel(query, state.Dependency); 88 | } 89 | 90 | [BurstCompile] 91 | private struct FindPathJob : IJobChunk 92 | { 93 | [ReadOnly] public NavMeshQuery NavMeshQuery; 94 | public NativeWorkQueue.ParallelWriter PathRequests; 95 | public EntityCommandBuffer.ParallelWriter CommandBuffer; 96 | 97 | [ReadOnly] public EntityTypeHandle Entities_ReadHandle; 98 | public ComponentTypeHandle FindPath_WriteHandle; 99 | 100 | [ReadOnly] public ComponentLookup LocalTransform_Lookup; 101 | 102 | public void Execute(in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask) 103 | { 104 | var entities = chunk.GetEntityDataPtrRO(Entities_ReadHandle); 105 | var findPaths = chunk.GetComponentDataPtrRW(ref FindPath_WriteHandle); 106 | 107 | for (int i =0; i < chunk.Count;i++) 108 | { 109 | ref var findPath = ref findPaths[i]; 110 | 111 | if (!LocalTransform_Lookup.TryGetComponent(findPath.source, out var sourceTransform)) 112 | { 113 | //Debug.Log("source dead -> destroy findPath entity"); 114 | var entity = entities[i]; 115 | CommandBuffer.DestroyEntity(unfilteredChunkIndex, entity); 116 | continue; 117 | } 118 | 119 | float3 sourcePos = sourceTransform.Position; 120 | float3 targetPos = findPath.target == Entity.Null ? findPath.targetPosition : LocalTransform_Lookup[findPath.target].Position; 121 | 122 | float length = math.distancesq(sourcePos, targetPos); 123 | if (length <= findPath.requiredMinDistanceSq) 124 | { 125 | // don't destroy, WalkPathSystem needs to give control back 126 | continue; 127 | } 128 | 129 | int pathId = PathRequests.TryAdd(out PathQuery* pathRequest); 130 | 131 | if (pathId == 0) 132 | { 133 | //Debug.LogError("Too much requests"); 134 | continue; 135 | } 136 | 137 | //Debug.Log($"Firing off request with id {pathId} from {sourcePos} to {targetPos}"); 138 | findPath.pathId = pathId; 139 | findPath.PathStatus = NZCore.Pathfinding.PathStatus.InProgress; 140 | findPath.pathWalkerIndex = 0; 141 | 142 | pathRequest->PathRequestId = pathId; 143 | pathRequest->From = NavMeshQuery.MapLocation(sourcePos, new float3(10, 10, 10), 0, 1 << 0); 144 | pathRequest->To = NavMeshQuery.MapLocation(targetPos, new float3(10, 10, 10), 0, 1 << 0); 145 | pathRequest->AreaMask = 1 << 0; 146 | pathRequest->Status = default; // todo, actual danger, not setting this screws up every request afterwards 147 | } 148 | } 149 | } 150 | } 151 | 152 | } 153 | -------------------------------------------------------------------------------- /Runtime/Systems/WalkPathSystem.cs: -------------------------------------------------------------------------------- 1 | using Unity.Burst; 2 | using Unity.Burst.Intrinsics; 3 | using Unity.Collections; 4 | using Unity.Entities; 5 | using Unity.Mathematics; 6 | using Unity.Physics.Authoring; 7 | using Unity.Transforms; 8 | 9 | namespace NZCore.Pathfinding 10 | { 11 | public struct PathFinderControlled : IComponentData, IEnableableComponent { } 12 | 13 | [BurstCompile] 14 | [UpdateInGroup(typeof(Stage1SystemGroup))] 15 | [UpdateAfter(typeof(PathfinderSystem))] 16 | public partial struct WalkPathSystem : ISystem 17 | { 18 | private EntityQuery query; 19 | 20 | [BurstCompile] 21 | public void OnCreate(ref SystemState state) 22 | { 23 | query = new EntityQueryBuilder(Allocator.Temp) 24 | .WithAll() 25 | .Build(ref state); 26 | } 27 | 28 | public void OnDestroy(ref SystemState state) 29 | { 30 | } 31 | 32 | [BurstCompile] 33 | public void OnUpdate(ref SystemState state) 34 | { 35 | var deltaTime = SystemAPI.Time.DeltaTime; 36 | var destroyCommandBuffer = SystemAPI.GetSingleton().CreateCommandBuffer(state.WorldUnmanaged); 37 | var pathResults = SystemAPI.GetSingleton(); 38 | 39 | var processHandle = new ProcessPathResults() 40 | { 41 | FindPath_ReadHandle = SystemAPI.GetComponentTypeHandle(true), 42 | PathBuffer_WriteHandle = SystemAPI.GetBufferTypeHandle(false), 43 | PathResults = pathResults.Results, 44 | 45 | }.ScheduleParallel(query, state.Dependency); 46 | 47 | state.Dependency = new WalkPathJob 48 | { 49 | commandBuffer = destroyCommandBuffer.AsParallelWriter(), 50 | deltaTime = deltaTime, 51 | Entities_ReadHandle = SystemAPI.GetEntityTypeHandle(), 52 | FindPath_WriteHandle = SystemAPI.GetComponentTypeHandle(false), 53 | PathBuffer_ReadHandle = SystemAPI.GetBufferTypeHandle(true), 54 | //PathFinderControlled_WriteHandle = SystemAPI.GetComponentTypeHandle(false), 55 | PathFinderControlled_WriteLookup = SystemAPI.GetComponentLookup(false), 56 | 57 | LocalTransform_WriteLookup = SystemAPI.GetComponentLookup(false), 58 | Velocity_WriteLookup = SystemAPI.GetComponentLookup(false) 59 | }.ScheduleParallel(query, processHandle); 60 | } 61 | 62 | [BurstCompile] 63 | public unsafe struct ProcessPathResults : IJobChunk 64 | { 65 | [ReadOnly] public ComponentTypeHandle FindPath_ReadHandle; 66 | 67 | [NativeDisableParallelForRestriction] 68 | public BufferTypeHandle PathBuffer_WriteHandle; 69 | 70 | [ReadOnly] public NativeParallelHashMap PathResults; 71 | 72 | public void Execute(in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask) 73 | { 74 | var findPaths = chunk.GetComponentDataPtrRO(ref FindPath_ReadHandle); 75 | var pathBufferAccessor = chunk.GetBufferAccessor(ref PathBuffer_WriteHandle); 76 | 77 | for (int i =0; i < chunk.Count;i++) 78 | { 79 | ref var findPath = ref findPaths[i]; 80 | 81 | if (findPath.pathId == 0 || !PathResults.ContainsKey(findPath.pathId)) 82 | continue; 83 | 84 | var result = PathResults[findPath.pathId]; 85 | findPath.PathStatus = PathStatus.Success; 86 | 87 | //Debug.Log($"Found path result writing {result.PathLength}"); 88 | 89 | var buffer = pathBufferAccessor[i]; 90 | buffer.Clear(); 91 | 92 | for (int ii = 1; ii < result.PathLength; ii++) 93 | { 94 | buffer.Add(new PathBuffer() 95 | { 96 | position = result.Path[ii].position 97 | }); 98 | 99 | PhysicsDebugDisplaySystem.Line(result.Path[ii - 1].position, result.Path[ii].position, Unity.DebugDisplay.ColorIndex.Green); 100 | } 101 | } 102 | } 103 | } 104 | 105 | [BurstCompile] 106 | public unsafe struct WalkPathJob : IJobChunk 107 | { 108 | [ReadOnly] public EntityTypeHandle Entities_ReadHandle; 109 | [ReadOnly] public BufferTypeHandle PathBuffer_ReadHandle; 110 | public ComponentTypeHandle FindPath_WriteHandle; 111 | //public ComponentTypeHandle PathFinderControlled_WriteHandle; 112 | 113 | [NativeDisableParallelForRestriction] public UnsafeComponentLookup PathFinderControlled_WriteLookup; 114 | [NativeDisableParallelForRestriction] public UnsafeComponentLookup LocalTransform_WriteLookup; 115 | [NativeDisableParallelForRestriction] public UnsafeComponentLookup Velocity_WriteLookup; 116 | 117 | public EntityCommandBuffer.ParallelWriter commandBuffer; 118 | public float deltaTime; 119 | 120 | public void Execute(in ArchetypeChunk chunk, int unfilteredChunkIndex, bool useEnabledMask, in v128 chunkEnabledMask) 121 | { 122 | var entities = chunk.GetNativeArray(Entities_ReadHandle); 123 | var pathBuffers = chunk.GetBufferAccessor(ref PathBuffer_ReadHandle); 124 | var findPaths = chunk.GetComponentDataPtrRW(ref FindPath_WriteHandle); 125 | 126 | for (int i = 0; i < chunk.Count; i++) 127 | { 128 | ref var findPath = ref findPaths[i]; 129 | if (findPath.PathStatus != PathStatus.Success) 130 | continue; 131 | 132 | var source = findPath.source; 133 | 134 | if (!LocalTransform_WriteLookup.HasComponent(source)) 135 | { 136 | // entity was destroyed 137 | //Debug.Log($"entity was destroyed"); 138 | commandBuffer.DestroyEntity(unfilteredChunkIndex, entities[i]); 139 | continue; 140 | } 141 | 142 | var pathBuffer = pathBuffers[i]; 143 | ref var velocity = ref Velocity_WriteLookup.GetRef(source, true); 144 | 145 | if (findPath.pathWalkerIndex >= pathBuffer.Length) 146 | { 147 | // reached destination 148 | //Debug.Log($"reached destination targetPos: {findPath.targetPosition}"); 149 | commandBuffer.DestroyEntity(unfilteredChunkIndex, entities[i]); 150 | 151 | PathFinderControlled_WriteLookup.SetComponentEnabled(source, false); 152 | velocity = default; 153 | //Debug.Log("Lost control is false"); 154 | continue; 155 | } 156 | 157 | ref var sourceTransform = ref LocalTransform_WriteLookup.GetRef(source, true); 158 | var direction = pathBuffer[findPath.pathWalkerIndex].position - sourceTransform.Position; 159 | var lengthToWaypoint = math.lengthsq(direction); 160 | var minimumMoveStep = findPath.speed * deltaTime; 161 | 162 | if (lengthToWaypoint < math.pow(minimumMoveStep, 2)) 163 | { 164 | findPath.pathWalkerIndex++; 165 | 166 | if (findPath.pathWalkerIndex < pathBuffer.Length) 167 | direction = pathBuffer[findPath.pathWalkerIndex].position - sourceTransform.Position; 168 | } 169 | 170 | var normalizedDir = math.normalizesafe(direction); 171 | //Debug.Log($"Direction {direction} speed: {findPath.speed}"); 172 | 173 | velocity.Value = normalizedDir * findPath.speed; 174 | sourceTransform.Position += normalizedDir * minimumMoveStep; 175 | sourceTransform.Rotation = quaternion.LookRotation(normalizedDir, new float3(0, 1, 0)); 176 | 177 | //Debug.Log($"Walkpath moving by {(normalizedDir * maximumMoveLength)}"); 178 | PathFinderControlled_WriteLookup.SetComponentEnabled(source, true); 179 | } 180 | } 181 | } 182 | } 183 | } 184 | -------------------------------------------------------------------------------- /Runtime/Systems/PathfinderSystem.cs: -------------------------------------------------------------------------------- 1 | using System; 2 | using Unity.Burst; 3 | using Unity.Collections; 4 | using Unity.Collections.LowLevel.Unsafe; 5 | using Unity.Entities; 6 | using Unity.Jobs; 7 | using Unity.Mathematics; 8 | using UnityEngine.Experimental.AI; 9 | 10 | namespace NZCore.Pathfinding 11 | { 12 | public enum PathStatus 13 | { 14 | None, 15 | InProgress, 16 | Success, 17 | Failed 18 | } 19 | 20 | public unsafe struct PathQueryResult 21 | { 22 | public NavMeshLocation* Path; 23 | public int PathLength; 24 | //public PathStatus Status; 25 | } 26 | 27 | public unsafe struct PathQueryPtr 28 | { 29 | public PathQuery* PathQuery; 30 | } 31 | 32 | public unsafe struct PathQuery 33 | { 34 | public int PathRequestId; 35 | 36 | public NavMeshLocation From; 37 | public NavMeshLocation To; 38 | 39 | //public int id; 40 | //public int key; 41 | public int AreaMask; 42 | 43 | // result data 44 | public NavMeshLocation* Path; 45 | public int PathLength; 46 | public PathQueryStatus Status; 47 | 48 | } 49 | 50 | public struct PathRequestsSingleton : IComponentData 51 | { 52 | public NativeWorkQueue Requests; 53 | } 54 | 55 | public struct PathResultsSingleton : IComponentData 56 | { 57 | public NativeParallelHashMap Results; 58 | } 59 | 60 | [UpdateInGroup(typeof(Stage1SystemGroup))] 61 | [UpdateAfter(typeof(FindPathSystem))] 62 | public partial struct PathfinderSystem : ISystem 63 | { 64 | private const int MAX_WORKERS = 100; 65 | private const int QUERY_MAX_COUNT = 100; 66 | private const int PATH_MAX_SIZE = 512; 67 | private const int MAX_PATH_QUEUE_NODES = 4096; 68 | 69 | private NavMeshWorld _navMeshWorld; 70 | private NativeArray _queries; 71 | 72 | private NativeWorkQueue _pathRequests; // only for alloc/dispose 73 | private NativeArray _inProgress; 74 | 75 | private NativeParallelHashMap _pathResults; // only for alloc/dispose 76 | 77 | 78 | [BurstCompile] 79 | public unsafe void OnCreate(ref SystemState state) 80 | { 81 | _navMeshWorld = NavMeshWorld.GetDefaultWorld(); 82 | 83 | _pathRequests = new NativeWorkQueue(QUERY_MAX_COUNT, Allocator.Persistent); 84 | _inProgress = new NativeArray(QUERY_MAX_COUNT, Allocator.Persistent); 85 | 86 | _pathResults = new NativeParallelHashMap(QUERY_MAX_COUNT, Allocator.Persistent); 87 | 88 | for (int i = 0; i < QUERY_MAX_COUNT; i++) 89 | { 90 | ref var entry = ref _pathRequests.ElementAt(i); 91 | entry.Path = (NavMeshLocation*) UnsafeUtility.Malloc(UnsafeUtility.SizeOf() * PATH_MAX_SIZE, UnsafeUtility.AlignOf(), Allocator.Persistent); 92 | entry.Status = 0; 93 | 94 | _inProgress[i] = new PathQueryPtr() 95 | { 96 | PathQuery = null 97 | }; 98 | } 99 | 100 | state.EntityManager.AddComponentData(state.SystemHandle, new PathRequestsSingleton() 101 | { 102 | Requests = _pathRequests 103 | }); 104 | state.EntityManager.AddComponentData(state.SystemHandle, new PathResultsSingleton() 105 | { 106 | Results = _pathResults 107 | }); 108 | 109 | _queries = new NativeArray(MAX_WORKERS, Allocator.Persistent); 110 | 111 | for (int i = 0; i < MAX_WORKERS; i++) 112 | { 113 | _queries[i] = new NavMeshQuery(_navMeshWorld, Allocator.Persistent, MAX_PATH_QUEUE_NODES); 114 | } 115 | 116 | SystemAPI.GetSingletonRW(); 117 | } 118 | 119 | [BurstCompile] 120 | public unsafe void OnDestroy(ref SystemState state) 121 | { 122 | for (int i = 0; i < QUERY_MAX_COUNT; i++) 123 | { 124 | UnsafeUtility.Free(_pathRequests.ElementAt(i).Path, Allocator.Persistent); 125 | } 126 | 127 | _pathRequests.Dispose(); 128 | 129 | //results.Dispose(); 130 | 131 | for (int i = 0; i < MAX_WORKERS; i++) 132 | { 133 | _queries[i].Dispose(); 134 | } 135 | 136 | _queries.Dispose(); 137 | 138 | _pathResults.Dispose(); 139 | _inProgress.Dispose(); 140 | 141 | } 142 | 143 | [BurstCompile] 144 | public void OnUpdate(ref SystemState state) 145 | { 146 | var requests = SystemAPI.GetSingleton(); 147 | var results = SystemAPI.GetSingleton(); 148 | 149 | var clearHandle = new ClearResultsHashMap() 150 | { 151 | Results = results.Results 152 | }.Schedule(state.Dependency); 153 | 154 | state.Dependency = new ProcessQueues() 155 | { 156 | MaxIters = 4096, 157 | NavMeshQueries = _queries, 158 | PathRequests = requests.Requests.AsParallelReader(), 159 | InProgress = _inProgress, 160 | Results = results.Results.AsParallelWriter() 161 | //}.ScheduleParallel(MAX_WORKERS, 1, state.Dependency); 162 | }.Schedule(MAX_WORKERS, clearHandle); 163 | } 164 | 165 | [BurstCompile] 166 | private struct ClearResultsHashMap : IJob 167 | { 168 | public NativeParallelHashMap Results; 169 | 170 | public void Execute() 171 | { 172 | Results.Clear(); 173 | } 174 | } 175 | 176 | [BurstCompile] 177 | private unsafe struct ProcessQueues : IJobFor 178 | { 179 | public int MaxIters; 180 | 181 | [ReadOnly] 182 | [NativeDisableContainerSafetyRestriction] 183 | public NativeArray NavMeshQueries; 184 | 185 | public NativeWorkQueue.ParallelReader PathRequests; 186 | public NativeArray InProgress; 187 | 188 | public NativeParallelHashMap.ParallelWriter Results; 189 | 190 | public void Execute(int index) 191 | { 192 | int iterCount = MaxIters; 193 | var navMeshQuery = NavMeshQueries[index]; 194 | 195 | if (InProgress[index].PathQuery != null) 196 | { 197 | //ref var previousQuery = ref UnsafeUtility.AsRef(InProgress[index].PathQuery); 198 | ref var previousQuery = ref *InProgress[index].PathQuery; 199 | 200 | if (!ProcessElement(ref previousQuery, ref navMeshQuery, ref iterCount)) 201 | { 202 | // Didn't finish processing the work 203 | return; 204 | } 205 | } 206 | 207 | //ref var previousQuery = ref InProgress.ElementAt(index); 208 | 209 | if (PathRequests.TryGetNext(out PathQuery* pathQuery)) 210 | { 211 | if (!ProcessElement(ref *pathQuery, ref navMeshQuery, ref iterCount)) 212 | { 213 | // Didn't finish processing the work, store it for later 214 | InProgress[pathQuery->PathRequestId] = new PathQueryPtr() 215 | { 216 | PathQuery = pathQuery 217 | }; 218 | return; 219 | } 220 | } 221 | 222 | var ptr = (PathQueryPtr*) InProgress.GetUnsafePtr(); 223 | ref var element = ref UnsafeUtility.AsRef(ptr + index); 224 | element.PathQuery = null; 225 | } 226 | 227 | // True if we should continue processing 228 | private bool ProcessElement(ref PathQuery query, ref NavMeshQuery navMeshQuery, ref int iterCount) 229 | { 230 | if (iterCount <= 0) 231 | { 232 | return false; 233 | } 234 | 235 | var result = Process(ref query, ref navMeshQuery, ref iterCount); 236 | if (!result) 237 | { 238 | return false; 239 | } 240 | 241 | // Write our result 242 | //Debug.Log($"Write path results from id {query.PathRequestId} length {query.PathLength}"); 243 | Results.TryAdd(query.PathRequestId, new PathQueryResult 244 | { 245 | //Status = *query.Status, 246 | Path = query.Path, 247 | PathLength = query.PathLength, 248 | }); 249 | //Check.Assume(added); 250 | return true; 251 | } 252 | 253 | private bool Process(ref PathQuery query, ref NavMeshQuery navMeshQuery, ref int iterCount) 254 | { 255 | // Handle query start. 256 | if (query.Status == 0) 257 | { 258 | query.Status = navMeshQuery.BeginFindPath(query.From, query.To, query.AreaMask, default); 259 | } 260 | 261 | // Handle query in progress. 262 | if (query.Status == PathQueryStatus.InProgress) 263 | { 264 | query.Status = navMeshQuery.UpdateFindPath(iterCount, out var iterationsPerformed); 265 | iterCount -= iterationsPerformed; 266 | } 267 | 268 | // Check query complete 269 | if (query.Status == PathQueryStatus.Success) 270 | { 271 | var unusedResult = navMeshQuery.EndFindPath(out int pathLength); 272 | 273 | var polygons = new NativeArray(pathLength, Allocator.Temp); 274 | navMeshQuery.GetPathResult(polygons); 275 | 276 | var straightPathFlags = new NativeArray(PATH_MAX_SIZE, Allocator.Temp); 277 | var vertexSide = new NativeArray(PATH_MAX_SIZE, Allocator.Temp); 278 | 279 | var cornerCount = 0; 280 | 281 | query.Status = FindStraightPath(ref navMeshQuery, 282 | query.From.position, 283 | query.To.position, 284 | pathLength, 285 | polygons, 286 | query.Path, 287 | ref straightPathFlags, 288 | ref vertexSide, 289 | ref cornerCount 290 | ); 291 | 292 | query.PathLength = cornerCount; 293 | 294 | return true; 295 | } 296 | 297 | // We've still finished, we just failed in our query 298 | if (query.Status == PathQueryStatus.Failure) 299 | { 300 | //Debug.Log($"PathQueryStatus.Failure id {query.PathRequestId}"); 301 | return true; 302 | } 303 | 304 | return false; 305 | } 306 | 307 | private PathQueryStatus FindStraightPath( 308 | ref NavMeshQuery query, 309 | float3 startPos, 310 | float3 endPos, 311 | int pathSize, 312 | NativeArray polygons, 313 | NavMeshLocation* straightPath, 314 | ref NativeArray straightPathFlags, 315 | ref NativeArray vertexSide, 316 | ref int straightPathCount) 317 | { 318 | if (!query.IsValid(polygons[0])) 319 | { 320 | //Debug.Log("Query Failure 1"); 321 | straightPath[0] = new NavMeshLocation (); // empty terminator 322 | return PathQueryStatus.Failure; // | PathQueryStatus.InvalidParam; 323 | } 324 | 325 | straightPath[0] = query.CreateLocation (startPos, polygons[0]); 326 | 327 | straightPathFlags[0] = StraightPathFlags.Start; 328 | 329 | var apexIndex = 0; 330 | var n = 1; 331 | 332 | if (pathSize > 1) 333 | { 334 | var startPolyWorldToLocal = query.PolygonWorldToLocalMatrix(polygons[0]); 335 | 336 | float3 apex = startPolyWorldToLocal.MultiplyPoint(startPos); 337 | var left = new float3(0, 0, 0); // Vector3.zero accesses a static readonly which does not work in burst yet 338 | var right = new float3(0, 0, 0); 339 | var leftIndex = -1; 340 | var rightIndex = -1; 341 | 342 | for (var i = 1; i <= pathSize; ++i) 343 | { 344 | var polyWorldToLocal = query.PolygonWorldToLocalMatrix(polygons[apexIndex]); 345 | 346 | float3 vl, vr; 347 | if (i == pathSize) 348 | { 349 | vl = vr = polyWorldToLocal.MultiplyPoint(endPos); 350 | 351 | } 352 | else 353 | { 354 | var success = query.GetPortalPoints(polygons[i - 1], polygons[i], out var vecvl, out var vecvr); 355 | if (!success) 356 | { 357 | //Debug.Log("Query Failure 2"); 358 | return PathQueryStatus.Failure; // | PathQueryStatus.InvalidParam; 359 | } 360 | 361 | vl = polyWorldToLocal.MultiplyPoint(vecvl); 362 | vr = polyWorldToLocal.MultiplyPoint(vecvr); 363 | } 364 | 365 | vl -= apex; 366 | vr -= apex; 367 | 368 | // Ensure left/right ordering 369 | if (Perp2D(vl, vr) < 0) 370 | Swap(ref vl, ref vr); 371 | 372 | // Terminate funnel by turning 373 | if (Perp2D(left, vr) < 0) 374 | { 375 | var polyLocalToWorld = query.PolygonLocalToWorldMatrix(polygons[apexIndex]); 376 | var termPos = polyLocalToWorld.MultiplyPoint(apex + left); 377 | 378 | n = RetracePortals(ref query, apexIndex, leftIndex, n, termPos, polygons, straightPath, ref straightPathFlags); 379 | if (vertexSide.Length > 0) 380 | { 381 | vertexSide[n - 1] = -1; 382 | } 383 | 384 | //Debug.Log("LEFT"); 385 | 386 | if (n == PATH_MAX_SIZE) 387 | { 388 | straightPathCount = n; 389 | return PathQueryStatus.Success; // | PathQueryStatus.BufferTooSmall; 390 | } 391 | 392 | apex = polyWorldToLocal.MultiplyPoint(termPos); 393 | left = float3.zero; 394 | right = float3.zero; 395 | i = apexIndex = leftIndex; 396 | continue; 397 | } 398 | 399 | if (Perp2D(right, vl) > 0) 400 | { 401 | var polyLocalToWorld = query.PolygonLocalToWorldMatrix(polygons[apexIndex]); 402 | var termPos = polyLocalToWorld.MultiplyPoint(apex + right); 403 | 404 | n = RetracePortals(ref query, apexIndex, rightIndex, n, termPos, polygons, straightPath, ref straightPathFlags); 405 | if (vertexSide.Length > 0) 406 | { 407 | vertexSide[n - 1] = 1; 408 | } 409 | 410 | //Debug.Log("RIGHT"); 411 | 412 | if (n == PATH_MAX_SIZE) 413 | { 414 | straightPathCount = n; 415 | return PathQueryStatus.Success; // | PathQueryStatus.BufferTooSmall; 416 | } 417 | 418 | apex = polyWorldToLocal.MultiplyPoint(termPos); 419 | left = float3.zero; 420 | right = float3.zero; 421 | i = apexIndex = rightIndex; 422 | continue; 423 | } 424 | 425 | // Narrow funnel 426 | if (Perp2D(left, vl) >= 0) 427 | { 428 | left = vl; 429 | leftIndex = i; 430 | } 431 | 432 | if (Perp2D(right, vr) <= 0) 433 | { 434 | right = vr; 435 | rightIndex = i; 436 | } 437 | } 438 | } 439 | 440 | // Remove the the next to last if duplicate point - e.g. start and end positions are the same 441 | // (in which case we have get a single point) 442 | if (n > 0 && math.all(((float3)straightPath[n - 1].position == endPos))) 443 | n--; 444 | 445 | n = RetracePortals(ref query, apexIndex, pathSize - 1, n, endPos, polygons, straightPath, ref straightPathFlags); 446 | if (vertexSide.Length > 0) 447 | { 448 | vertexSide[n - 1] = 0; 449 | } 450 | 451 | if (n == PATH_MAX_SIZE) 452 | { 453 | straightPathCount = n; 454 | return PathQueryStatus.Success; // | PathQueryStatus.BufferTooSmall; 455 | } 456 | 457 | // Fix flag for final path point 458 | straightPathFlags[n - 1] = StraightPathFlags.End; 459 | 460 | straightPathCount = n; 461 | return PathQueryStatus.Success; 462 | } 463 | 464 | private int RetracePortals( 465 | ref NavMeshQuery query, 466 | int startIndex, 467 | int endIndex, 468 | int n, 469 | float3 termPos, 470 | NativeArray path, 471 | NavMeshLocation* straightPath, 472 | ref NativeArray straightPathFlags) 473 | { 474 | for (var k = startIndex; k < endIndex - 1; ++k) 475 | { 476 | var type1 = query.GetPolygonType(path[k]); 477 | var type2 = query.GetPolygonType(path[k + 1]); 478 | 479 | if (type1 != type2) 480 | { 481 | float3 cpa1, cpa2; 482 | var status = query.GetPortalPoints(path[k], path[k + 1], out var vecl, out var vecr); 483 | 484 | SegmentSegmentCPA(out cpa1, out cpa2, vecl, vecr, straightPath[n - 1].position, termPos); 485 | straightPath[n] = query.CreateLocation(cpa1, path[k + 1]); 486 | 487 | straightPathFlags[n] = (type2 == NavMeshPolyTypes.OffMeshConnection) ? StraightPathFlags.OffMeshConnection : 0; 488 | if (++n == PATH_MAX_SIZE) 489 | { 490 | return PATH_MAX_SIZE; 491 | } 492 | } 493 | } 494 | 495 | straightPath[n] = query.CreateLocation(termPos, path[endIndex]); 496 | straightPathFlags[n] = query.GetPolygonType(path[endIndex]) == NavMeshPolyTypes.OffMeshConnection ? StraightPathFlags.OffMeshConnection : 0; 497 | return ++n; 498 | } 499 | 500 | private float Perp2D(float3 u, float3 v) 501 | { 502 | return u.z * v.x - u.x * v.z; 503 | } 504 | 505 | private void Swap (ref float3 a, ref float3 b) 506 | { 507 | (a, b) = (b, a); 508 | } 509 | 510 | private bool SegmentSegmentCPA (out float3 c0, out float3 c1, float3 p0, float3 p1, float3 q0, float3 q1) 511 | { 512 | var u = p1 - p0; 513 | var v = q1 - q0; 514 | var w0 = p0 - q0; 515 | 516 | float a = math.dot (u, u); 517 | float b = math.dot (u, v); 518 | float c = math.dot (v, v); 519 | float d = math.dot (u, w0); 520 | float e = math.dot (v, w0); 521 | 522 | float den = (a * c - b * b); 523 | float sc, tc; 524 | 525 | if (den == 0) 526 | { 527 | sc = 0; 528 | tc = d / b; 529 | 530 | // todo: handle b = 0 (=> a and/or c is 0) 531 | } 532 | else 533 | { 534 | sc = (b * e - c * d) / (a * c - b * b); 535 | tc = (a * e - b * d) / (a * c - b * b); 536 | } 537 | 538 | c0 = math.lerp (p0, p1, sc); 539 | c1 = math.lerp (q0, q1, tc); 540 | 541 | return den != 0; 542 | } 543 | } 544 | 545 | [Flags] 546 | public enum StraightPathFlags 547 | { 548 | Start = 0x01, // The vertex is the start position. 549 | End = 0x02, // The vertex is the end position. 550 | OffMeshConnection = 0x04 // The vertex is start of an off-mesh link. 551 | } 552 | } 553 | } --------------------------------------------------------------------------------