diff options
Diffstat (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding')
20 files changed, 3646 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/BlockableChannel.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/BlockableChannel.cs new file mode 100644 index 0000000..e3fce03 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/BlockableChannel.cs @@ -0,0 +1,212 @@ +using System.Threading; +using Pathfinding.Util; +using UnityEngine.Assertions; + +namespace Pathfinding { + /// <summary> + /// Multi-producer-multi-consumer (MPMC) channel. + /// + /// This is a channel that can be used to send data between threads. + /// It is thread safe and can be used by multiple threads at the same time. + /// + /// Additionally, the channel can be put into a blocking mode, which will cause all calls to Receive to block until the channel is unblocked. + /// </summary> + internal class BlockableChannel<T> where T : class { + public enum PopState { + Ok, + Wait, + Closed, + } + + readonly System.Object lockObj = new System.Object(); + + CircularBuffer<T> queue = new CircularBuffer<T>(16); + public int numReceivers { get; private set; } + + // Marked as volatile such that the compiler will not try to optimize the allReceiversBlocked property too much (this is more of a theoretical concern than a practical issue). + volatile int waitingReceivers; +#if !UNITY_WEBGL + ManualResetEvent starving = new ManualResetEvent(false); +#endif + bool blocked; + + /// <summary>True if <see cref="Close"/> has been called</summary> + public bool isClosed { get; private set; } + + /// <summary>True if the queue is empty</summary> + public bool isEmpty { + get { + lock (lockObj) { + return queue.Length == 0; + } + } + } + + /// <summary>True if blocking and all receivers are waiting for unblocking</summary> + // Note: This is designed to be lock-free for performance. But it will only generate a useful value if called from the same thread that is blocking/unblocking the queue, otherwise the return value could become invalid at any time. + public bool allReceiversBlocked => blocked && waitingReceivers == numReceivers; + + /// <summary>If true, all calls to Receive will block until this property is set to false</summary> + public bool isBlocked { + get => blocked; + set { + lock (lockObj) { + blocked = value; + if (isClosed) return; + isStarving = value || queue.Length == 0; + } + } + } + + /// <summary>All calls to Receive and ReceiveNoBlock will now return PopState.Closed</summary> + public void Close () { + lock (lockObj) { + isClosed = true; + isStarving = false; + } + } + + bool isStarving { + get { +#if UNITY_WEBGL + // In WebGL, semaphores are not supported. + // They will compile, but they don't work properly. + // So instead we directly use what the starving semaphore should indicate. + return (blocked || queue.Length == 0) && !isClosed; +#else + return !starving.WaitOne(0); +#endif + } + set { +#if !UNITY_WEBGL + if (value) starving.Reset(); + else starving.Set(); +#endif + } + } + + /// <summary> + /// Resets a closed channel so that it can be used again. + /// + /// The existing queue is preserved. + /// + /// This will throw an exception if there are any receivers still active. + /// </summary> + public void Reopen () { + lock (lockObj) { + if (numReceivers != 0) throw new System.InvalidOperationException("Can only reopen a channel after Close has been called on all receivers"); + Assert.AreEqual(waitingReceivers, 0); + isClosed = false; + isBlocked = false; + } + } + + public Receiver AddReceiver () { + lock (lockObj) { + if (isClosed) throw new System.InvalidOperationException("Channel is closed"); + this.numReceivers++; + return new Receiver(this); + } + } + + /// <summary>Push a path to the front of the queue</summary> + public void PushFront (T path) { + lock (lockObj) { + if (isClosed) throw new System.InvalidOperationException("Channel is closed"); + queue.PushStart(path); + if (!blocked) isStarving = false; + } + } + + /// <summary>Push a path to the end of the queue</summary> + public void Push (T path) { + lock (lockObj) { + if (isClosed) throw new System.InvalidOperationException("Channel is closed"); + queue.PushEnd(path); + if (!blocked) isStarving = false; + } + } + + /// <summary>Allows receiving items from a channel</summary> + public struct Receiver { + BlockableChannel<T> channel; + + public Receiver(BlockableChannel<T> channel) { + this.channel = channel; + } + + /// <summary> + /// Call when a receiver was terminated. + /// + /// After this call, this receiver cannot be used for anything. + /// </summary> + public void Close () { + lock (channel.lockObj) { + Assert.IsTrue(channel.numReceivers > 0); + Assert.IsTrue(channel.waitingReceivers < channel.numReceivers); + channel.numReceivers--; + } + channel = null; + } + + /// <summary> + /// Receives the next item from the channel. + /// This call will block if there are no items in the channel or if the channel is currently blocked. + /// + /// Returns: PopState.Ok and a non-null item in the normal case. Returns PopState.Closed if the channel has been closed. + /// </summary> + public PopState Receive (out T item) { +#if UNITY_WEBGL + throw new System.Exception("Cannot block in WebGL. Use ReceiveNoBlock instead."); +#else + Interlocked.Increment(ref channel.waitingReceivers); + while (true) { + channel.starving.WaitOne(); + // Note that right here, the channel could become blocked, closed or anything really. We have to check conditions again after locking. + lock (channel.lockObj) { + if (channel.isClosed) { + Interlocked.Decrement(ref channel.waitingReceivers); + item = null; + return PopState.Closed; + } + if (channel.queue.Length == 0) channel.isStarving = true; + if (channel.isStarving) continue; + Assert.IsFalse(channel.blocked); + Interlocked.Decrement(ref channel.waitingReceivers); + item = channel.queue.PopStart(); + return PopState.Ok; + } + } +#endif + } + + /// <summary> + /// Receives the next item from the channel, this call will not block. + /// To ensure a consistent state, the caller must follow this pattern. + /// 1. Call ReceiveNoBlock(false), if PopState.Wait is returned, wait for a bit (e.g yield return null in a Unity coroutine) + /// 2. try again with PopNoBlock(true), if PopState.Wait, wait for a bit + /// 3. Repeat from step 2. + /// </summary> + public PopState ReceiveNoBlock (bool blockedBefore, out T item) { + item = null; + if (!blockedBefore) Interlocked.Increment(ref channel.waitingReceivers); + while (true) { + if (channel.isStarving) return PopState.Wait; + // Note that right here, the channel could become blocked, closed or anything really. We have to check conditions again after locking. + lock (channel.lockObj) { + if (channel.isClosed) { + Interlocked.Decrement(ref channel.waitingReceivers); + return PopState.Closed; + } + if (channel.queue.Length == 0) channel.isStarving = true; + if (channel.isStarving) continue; + Assert.IsFalse(channel.blocked); + Interlocked.Decrement(ref channel.waitingReceivers); + item = channel.queue.PopStart(); + return PopState.Ok; + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/BlockableChannel.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/BlockableChannel.cs.meta new file mode 100644 index 0000000..cad1e5f --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/BlockableChannel.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e6a8344fd0d1c453cbaf5c5eb8f55ca5 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GlobalNodeStorage.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GlobalNodeStorage.cs new file mode 100644 index 0000000..ec8b530 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GlobalNodeStorage.cs @@ -0,0 +1,299 @@ +using Unity.Collections; +using UnityEngine; +using Unity.Mathematics; +using Unity.Jobs; +using UnityEngine.Profiling; + +namespace Pathfinding { + using Pathfinding.Util; + using Pathfinding.Jobs; + + internal class GlobalNodeStorage { + readonly AstarPath astar; + Unity.Jobs.JobHandle lastAllocationJob; + + /// <summary> + /// Holds the next node index which has not been used by any previous node. + /// See: <see cref="nodeIndexPools"/> + /// </summary> + public uint nextNodeIndex = 1; + + /// <summary> + /// The number of nodes for which path node data has been reserved. + /// Will be at least as high as <see cref="nextNodeIndex"/> + /// </summary> + uint reservedPathNodeData = 0; + + /// <summary>Number of nodes that have been destroyed in total</summary> + public uint destroyedNodesVersion { get; private set; } + +#if ASTAR_MORE_MULTI_TARGET_PATH_TARGETS + public const int MaxTemporaryNodes = 4096; +#else + public const int MaxTemporaryNodes = 256; +#endif + + /// <summary> + /// Holds indices for nodes that have been destroyed. + /// To avoid trashing a lot of memory structures when nodes are + /// frequently deleted and created, node indices are reused. + /// + /// There's one pool for each possible number of node variants (1, 2 and 3). + /// </summary> + readonly IndexedStack<uint>[] nodeIndexPools = new [] { + new IndexedStack<uint>(), + new IndexedStack<uint>(), + new IndexedStack<uint>(), + }; + + public PathfindingThreadData[] pathfindingThreadData = new PathfindingThreadData[0]; + + /// <summary>Maps from NodeIndex to node</summary> + GraphNode[] nodes = new GraphNode[0]; + + public GlobalNodeStorage (AstarPath astar) { + this.astar = astar; + } + + public GraphNode GetNode(uint nodeIndex) => nodes[nodeIndex]; + +#if UNITY_EDITOR + public struct DebugPathNode { + public uint g; + public uint h; + public uint parentIndex; + public ushort pathID; + public byte fractionAlongEdge; + } +#endif + + public struct PathfindingThreadData { + public UnsafeSpan<PathNode> pathNodes; +#if UNITY_EDITOR + public UnsafeSpan<DebugPathNode> debugPathNodes; +#endif + } + + class IndexedStack<T> { + T[] buffer = new T[4]; + + public int Count { get; private set; } + + public void Push (T v) { + if (Count == buffer.Length) { + Util.Memory.Realloc(ref buffer, buffer.Length * 2); + } + + buffer[Count] = v; + Count++; + } + + public void Clear () { + Count = 0; + } + + public T Pop () { + Count--; + return buffer[Count]; + } + + /// <summary>Pop the last N elements and store them in the buffer. The items will be in insertion order.</summary> + public void PopMany (T[] resultBuffer, int popCount) { + if (popCount > Count) throw new System.IndexOutOfRangeException(); + System.Array.Copy(buffer, Count - popCount, resultBuffer, 0, popCount); + Count -= popCount; + } + } + + void DisposeThreadData () { + if (pathfindingThreadData.Length > 0) { + for (int i = 0; i < pathfindingThreadData.Length; i++) { + unsafe { + pathfindingThreadData[i].pathNodes.Free(Allocator.Persistent); +#if UNITY_EDITOR + pathfindingThreadData[i].debugPathNodes.Free(Allocator.Persistent); +#endif + } + } + pathfindingThreadData = new PathfindingThreadData[0]; + } + } + + public void SetThreadCount (int threadCount) { + if (pathfindingThreadData.Length != threadCount) { + DisposeThreadData(); + pathfindingThreadData = new PathfindingThreadData[threadCount]; + + for (int i = 0; i < pathfindingThreadData.Length; i++) { + // Allocate per-thread data. + // We allocate using UnsafeSpans because this code may run inside jobs, and Unity does not allow us to allocate NativeArray memory + // using the persistent allocator inside jobs. + pathfindingThreadData[i].pathNodes = new UnsafeSpan<PathNode>(Allocator.Persistent, (int)reservedPathNodeData + MaxTemporaryNodes); +#if UNITY_EDITOR + pathfindingThreadData[i].debugPathNodes = new UnsafeSpan<DebugPathNode>(Allocator.Persistent, (int)reservedPathNodeData); + pathfindingThreadData[i].debugPathNodes.FillZeros(); +#endif + var pnodes = pathfindingThreadData[i].pathNodes; + pnodes.Fill(PathNode.Default); + } + } + } + + /// <summary> + /// Initializes temporary path data for a node. + /// Warning: This method should not be called directly. + /// + /// See: <see cref="AstarPath.InitializeNode"/> + /// </summary> + public void InitializeNode (GraphNode node) { + var variants = node.PathNodeVariants; + + // Graphs may initialize nodes from different threads, + // so we need to lock. + // Luckily, uncontested locks are really really cheap in C# + lock (this) { + if (nodeIndexPools[variants-1].Count > 0) { + node.NodeIndex = nodeIndexPools[variants-1].Pop(); + } else { + // Highest node index in the new list of nodes + node.NodeIndex = nextNodeIndex; + nextNodeIndex += (uint)variants; + ReserveNodeIndices(nextNodeIndex); + } + + for (int i = 0; i < variants; i++) { + nodes[node.NodeIndex + i] = node; + } + + astar.hierarchicalGraph.OnCreatedNode(node); + } + } + + /// <summary> + /// Reserves space for global node data. + /// + /// Warning: Must be called only when a lock is held on this object. + /// </summary> + void ReserveNodeIndices (uint nextNodeIndex) { + if (nextNodeIndex <= reservedPathNodeData) return; + + reservedPathNodeData = math.ceilpow2(nextNodeIndex); + + // Allocate more internal pathfinding data for the new nodes + astar.hierarchicalGraph.ReserveNodeIndices(reservedPathNodeData); + var threadCount = pathfindingThreadData.Length; + DisposeThreadData(); + SetThreadCount(threadCount); + Memory.Realloc(ref nodes, (int)reservedPathNodeData); + } + + /// <summary> + /// Destroyes the given node. + /// This is to be called after the node has been disconnected from the graph so that it cannot be reached from any other nodes. + /// It should only be called during graph updates, that is when the pathfinding threads are either not running or paused. + /// + /// Warning: This method should not be called by user code. It is used internally by the system. + /// </summary> + public void DestroyNode (GraphNode node) { + var nodeIndex = node.NodeIndex; + if (nodeIndex == GraphNode.DestroyedNodeIndex) return; + + destroyedNodesVersion++; + int variants = node.PathNodeVariants; + nodeIndexPools[variants - 1].Push(nodeIndex); + for (int i = 0; i < variants; i++) { + nodes[nodeIndex + i] = null; + } + + for (int t = 0; t < pathfindingThreadData.Length; t++) { + var threadData = pathfindingThreadData[t]; + for (uint i = 0; i < variants; i++) { + // This is not required for pathfinding, but not clearing it may confuse gizmo drawing for a fraction of a second. + // Especially when 'Show Search Tree' is enabled + threadData.pathNodes[nodeIndex + i].pathID = 0; +#if UNITY_EDITOR + threadData.debugPathNodes[nodeIndex + i].pathID = 0; +#endif + } + } + + astar.hierarchicalGraph.OnDestroyedNode(node); + } + + public void OnDisable () { + lastAllocationJob.Complete(); + nextNodeIndex = 1; + reservedPathNodeData = 0; + for (int i = 0; i < nodeIndexPools.Length; i++) nodeIndexPools[i].Clear(); + nodes = new GraphNode[0]; + DisposeThreadData(); + } + + struct JobAllocateNodes<T> : IJob where T : GraphNode { + public T[] result; + public int count; + public GlobalNodeStorage nodeStorage; + public uint variantsPerNode; + public System.Func<T> createNode; + + public bool allowBoundsChecks => false; + + public void Execute () { + Profiler.BeginSample("Allocating nodes"); + var hierarchicalGraph = nodeStorage.astar.hierarchicalGraph; + + lock (nodeStorage) { + var pool = nodeStorage.nodeIndexPools[variantsPerNode-1]; + uint nextNodeIndex = nodeStorage.nextNodeIndex; + + // Allocate the actual nodes + for (uint i = 0; i < count; i++) { + var node = result[i] = createNode(); + + // Get a new node index. Re-use one from a previously destroyed node if possible + if (pool.Count > 0) { + node.NodeIndex = pool.Pop(); + } else { + node.NodeIndex = nextNodeIndex; + nextNodeIndex += variantsPerNode; + } + } + + // Allocate more internal pathfinding data for the new nodes + nodeStorage.ReserveNodeIndices(nextNodeIndex); + + // Mark the node indices as used + nodeStorage.nextNodeIndex = nextNodeIndex; + + for (int i = 0; i < count; i++) { + var node = result[i]; + hierarchicalGraph.AddDirtyNode(node); + nodeStorage.nodes[node.NodeIndex] = node; + } + } + Profiler.EndSample(); + } + } + + public Unity.Jobs.JobHandle AllocateNodesJob<T>(T[] result, int count, System.Func<T> createNode, uint variantsPerNode) where T : GraphNode { + // Get all node indices that we are going to recycle and store them in a new buffer. + // It's best to store them in a new buffer to avoid multithreading issues. + UnityEngine.Assertions.Assert.IsTrue(variantsPerNode > 0 && variantsPerNode <= 3); + + // It may be tempting to use a parallel job for this + // but it seems like allocation (new) in C# uses some kind of locking. + // Therefore it is not faster (it may even be slower) to try to allocate the nodes in multiple threads in parallel. + // The job will use locking internally for safety, but it's still nice to set appropriate dependencies, to avoid lots of worker threads + // just stalling because they are waiting for a lock, in case this method is called multiple times in parallel. + lastAllocationJob = new JobAllocateNodes<T> { + result = result, + count = count, + nodeStorage = this, + variantsPerNode = variantsPerNode, + createNode = createNode, + }.ScheduleManaged(lastAllocationJob); + + return lastAllocationJob; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GlobalNodeStorage.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GlobalNodeStorage.cs.meta new file mode 100644 index 0000000..c348e27 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GlobalNodeStorage.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a659ff4e5955406479ba3f895ac8fca5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GraphUpdateProcessor.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GraphUpdateProcessor.cs new file mode 100644 index 0000000..16d5290 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GraphUpdateProcessor.cs @@ -0,0 +1,274 @@ +using System.Collections.Generic; +using UnityEngine; + +namespace Pathfinding { + using Pathfinding.Jobs; + using Pathfinding.Util; + using Unity.Jobs; + using Unity.Profiling; + using UnityEngine.Assertions; + using UnityEngine.Profiling; + + /// <summary> + /// Promise representing a graph update. + /// + /// This is used internally by the system to represent a graph update. + /// Generally you shouldn't need to care about it, unless you are implementing your own graph type. + /// </summary> + public interface IGraphUpdatePromise { + /// <summary> + /// Returns the progress of the update. + /// + /// This should be a value between 0 and 1. + /// </summary> + float Progress => 0.0f; + + /// <summary> + /// Coroutine to prepare an update asynchronously. + /// + /// If a JobHandle is returned, it will be awaited before the coroutine is ticked again, and before the <see cref="Apply"/> method is called. + /// + /// After this coroutine has finished, the <see cref="Apply"/> method will be called. + /// + /// Note: No changes must be made to the graph in this method. Those should only be done in the <see cref="Apply"/> method. + /// + /// May return null if no async work is required. + /// </summary> + IEnumerator<JobHandle> Prepare() => null; + + /// <summary> + /// Applies the update in a single atomic update. + /// + /// It is done as a single atomic update (from the main thread's perspective) to ensure + /// that even if one does an async scan or update, the graph will always be in a valid state. + /// This guarantees that things like GetNearest will still work during an async scan. + /// + /// Warning: Must only be called after the <see cref="Prepare"/> method has finished. + /// </summary> + // TODO: Pass in a JobHandle and allow returning a JobHandle? + void Apply(IGraphUpdateContext context); + } + + /// <summary> + /// Helper functions for graph updates. + /// + /// A context is passed to graphs when they are updated, and to work items when they are executed. + /// The <see cref="IWorkItemContext"/> interface inherits from this interface. + /// </summary> + public interface IGraphUpdateContext { + /// <summary> + /// Mark a particular region of the world as having been changed. + /// + /// This should be used whenever graphs are changed. + /// + /// This is used to recalculate off-mesh links that touch these bounds, and it will also ensure <see cref="GraphModifier"/> events are callled. + /// + /// The bounding box should cover the surface of all nodes that have been updated. + /// It is fine to use a larger bounding box than necessary (even an infinite one), though this may be slower, since more off-mesh links need to be recalculated. + /// You can even use an infinitely large bounding box if you don't want to bother calculating a more accurate one. + /// You can also call this multiple times to dirty multiple bounding boxes. + /// </summary> + void DirtyBounds(Bounds bounds); + } + + class GraphUpdateProcessor { + /// <summary>Holds graphs that can be updated</summary> + readonly AstarPath astar; + + /// <summary>Used for IsAnyGraphUpdateInProgress</summary> + bool anyGraphUpdateInProgress; + + /// <summary> + /// Queue containing all waiting graph update queries. Add to this queue by using \link AddToQueue \endlink. + /// See: AddToQueue + /// </summary> + readonly Queue<GraphUpdateObject> graphUpdateQueue = new Queue<GraphUpdateObject>(); + readonly List<(IGraphUpdatePromise, IEnumerator<JobHandle>)> pendingPromises = new List<(IGraphUpdatePromise, IEnumerator<JobHandle>)>(); + readonly List<GraphUpdateObject> pendingGraphUpdates = new List<GraphUpdateObject>(); + + /// <summary>Returns if any graph updates are waiting to be applied</summary> + public bool IsAnyGraphUpdateQueued { get { return graphUpdateQueue.Count > 0; } } + + /// <summary>Returns if any graph updates are in progress</summary> + public bool IsAnyGraphUpdateInProgress { get { return anyGraphUpdateInProgress; } } + + public GraphUpdateProcessor (AstarPath astar) { + this.astar = astar; + } + + /// <summary>Work item which can be used to apply all queued updates</summary> + public AstarWorkItem GetWorkItem () { + return new AstarWorkItem(QueueGraphUpdatesInternal, ProcessGraphUpdates); + } + + /// <summary> + /// Update all graphs using the GraphUpdateObject. + /// This can be used to, e.g make all nodes in an area unwalkable, or set them to a higher penalty. + /// The graphs will be updated as soon as possible (with respect to AstarPath.batchGraphUpdates) + /// + /// See: FlushGraphUpdates + /// </summary> + public void AddToQueue (GraphUpdateObject ob) { + // Put the GUO in the queue + graphUpdateQueue.Enqueue(ob); + } + + /// <summary> + /// Discards all queued graph updates. + /// + /// Graph updates that are already in progress will not be discarded. + /// </summary> + public void DiscardQueued () { + while (graphUpdateQueue.Count > 0) { + graphUpdateQueue.Dequeue().internalStage = GraphUpdateObject.STAGE_ABORTED; + } + } + + /// <summary>Schedules graph updates internally</summary> + void QueueGraphUpdatesInternal (IWorkItemContext context) { + Assert.IsTrue(pendingGraphUpdates.Count == 0); + while (graphUpdateQueue.Count > 0) { + var ob = graphUpdateQueue.Dequeue(); + pendingGraphUpdates.Add(ob); + if (ob.internalStage != GraphUpdateObject.STAGE_PENDING) { + Debug.LogError("Expected remaining graph update to be pending"); + continue; + } + } + + foreach (IUpdatableGraph g in astar.data.GetUpdateableGraphs()) { + NavGraph gr = g as NavGraph; + var updates = ListPool<GraphUpdateObject>.Claim(); + for (int i = 0; i < pendingGraphUpdates.Count; i++) { + GraphUpdateObject ob = pendingGraphUpdates[i]; + if (ob.nnConstraint == null || ob.nnConstraint.SuitableGraph((int)gr.graphIndex, gr)) { + updates.Add(ob); + } + } + if (updates.Count > 0) { + var promise = g.ScheduleGraphUpdates(updates); + if (promise != null) { + var it = promise.Prepare(); + pendingPromises.Add((promise, it)); + } else { + ListPool<GraphUpdateObject>.Release(ref updates); + } + } else { + ListPool<GraphUpdateObject>.Release(ref updates); + } + } + + context.PreUpdate(); + anyGraphUpdateInProgress = true; + } + + static readonly ProfilerMarker MarkerSleep = new ProfilerMarker(ProfilerCategory.Loading, "Sleep"); + static readonly ProfilerMarker MarkerCalculate = new ProfilerMarker("Calculating Graph Update"); + static readonly ProfilerMarker MarkerApply = new ProfilerMarker("Applying Graph Update"); + + /// <summary> + /// Updates graphs. + /// Will do some graph updates, possibly signal another thread to do them. + /// Will only process graph updates added by QueueGraphUpdatesInternal + /// + /// Returns: True if all graph updates have been done and pathfinding (or other tasks) may resume. + /// False if there are still graph updates being processed or waiting in the queue. + /// </summary> + /// <param name="context">Helper methods for the work items.</param> + /// <param name="force">If true, all graph updates will be processed before this function returns. The return value + /// will be True.</param> + bool ProcessGraphUpdates (IWorkItemContext context, bool force) { + Assert.IsTrue(anyGraphUpdateInProgress); + + if (pendingPromises.Count > 0) { + if (!ProcessGraphUpdatePromises(pendingPromises, context, force)) { + return false; + } + + pendingPromises.Clear(); + } + + anyGraphUpdateInProgress = false; + for (int i = 0; i < pendingGraphUpdates.Count; i++) { + pendingGraphUpdates[i].internalStage = GraphUpdateObject.STAGE_APPLIED; + } + pendingGraphUpdates.Clear(); + return true; + } + + public static bool ProcessGraphUpdatePromises (List<(IGraphUpdatePromise, IEnumerator<JobHandle>)> promises, IGraphUpdateContext context, bool force = false) { + TimeSlice timeSlice = TimeSlice.MillisFromNow(2f); + TimeSlice idleTimeSlice = default; + + while (true) { + int firstNonFinished = -1; + bool anyMainThreadProgress = false; + for (int i = 0; i < promises.Count; i++) { + var(promise, it) = promises[i]; + if (it == null) continue; + if (force) { + it.Current.Complete(); + } else { + if (it.Current.IsCompleted) { + it.Current.Complete(); + } else { + if (firstNonFinished == -1) firstNonFinished = i; + continue; + } + } + + anyMainThreadProgress = true; + MarkerCalculate.Begin(); + try { + if (it.MoveNext()) { + if (firstNonFinished == -1) firstNonFinished = i; + } else promises[i] = (promise, null); + } catch (System.Exception e) { + Debug.LogError(new System.Exception("Error while updating graphs.", e)); + promises[i] = (null, null); + } + MarkerCalculate.End(); + } + + if (firstNonFinished == -1) { + break; + } else if (!force) { + if (timeSlice.expired) { + return false; + } else if (anyMainThreadProgress) { + // Reset the idle time slice if we got something done on the main thread. + // This allows us to wait on more very short jobs. + idleTimeSlice = TimeSlice.MillisFromNow(0.1f); + } else if (idleTimeSlice.expired) { + return false; + } else { + // Allow waiting for a short amount of time to allow very short running + // jobs in graph updates to complete without having to wait until the next frame. + // While waiting we release our thread's time slice to make sure other threads get priority. + if (!anyMainThreadProgress) { + MarkerSleep.Begin(); + System.Threading.Thread.Yield(); + MarkerSleep.End(); + } + } + } + } + + for (int i = 0; i < promises.Count; i++) { + var(promise, it) = promises[i]; + Assert.IsNull(it); + if (promise != null) { + MarkerApply.Begin(); + try { + promise.Apply(context); + } catch (System.Exception e) { + Debug.LogError(new System.Exception("Error while updating graphs.", e)); + } + MarkerApply.End(); + } + } + + return true; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GraphUpdateProcessor.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GraphUpdateProcessor.cs.meta new file mode 100644 index 0000000..bd24da6 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/GraphUpdateProcessor.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: b1798d8e7c7d54972ae8522558cbd27c +timeCreated: 1443114816 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HeuristicObjective.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HeuristicObjective.cs new file mode 100644 index 0000000..2a2cea4 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HeuristicObjective.cs @@ -0,0 +1,99 @@ +using Unity.Mathematics; +using Unity.Burst; +using Pathfinding.Util; +using Pathfinding.Graphs.Util; + +namespace Pathfinding { + /// <summary> + /// Calculates an estimated cost from the specified point to the target. + /// + /// See: https://en.wikipedia.org/wiki/A*_search_algorithm + /// </summary> + [BurstCompile] + public readonly struct HeuristicObjective { + readonly int3 mn; + readonly int3 mx; + readonly Heuristic heuristic; + readonly float heuristicScale; + readonly UnsafeSpan<uint> euclideanEmbeddingCosts; + readonly uint euclideanEmbeddingPivots; + readonly uint targetNodeIndex; + + public HeuristicObjective (int3 point, Heuristic heuristic, float heuristicScale) { + this.mn = this.mx = point; + this.heuristic = heuristic; + this.heuristicScale = heuristicScale; + this.euclideanEmbeddingCosts = default; + this.euclideanEmbeddingPivots = 0; + this.targetNodeIndex = 0; + } + + public HeuristicObjective (int3 point, Heuristic heuristic, float heuristicScale, uint targetNodeIndex, EuclideanEmbedding euclideanEmbedding) { + this.mn = this.mx = point; + this.heuristic = heuristic; + this.heuristicScale = heuristicScale; + // The euclidean embedding costs are guaranteed to be valid for the duration of the pathfinding request. + // We cannot perform checks here, because we may be running in another thread, and Unity does not like that. + this.euclideanEmbeddingCosts = euclideanEmbedding != null? euclideanEmbedding.costs.AsUnsafeSpanNoChecks() : default; + this.euclideanEmbeddingPivots = euclideanEmbedding != null ? (uint)euclideanEmbedding.pivotCount : 0; + this.targetNodeIndex = targetNodeIndex; + } + + public HeuristicObjective (int3 mn, int3 mx, Heuristic heuristic, float heuristicScale, uint targetNodeIndex, EuclideanEmbedding euclideanEmbedding) { + this.mn = mn; + this.mx = mx; + this.heuristic = heuristic; + this.heuristicScale = heuristicScale; + // The euclidean embedding costs are guaranteed to be valid for the duration of the pathfinding request. + // We cannot perform checks here, because we may be running in another thread, and Unity does not like that. + this.euclideanEmbeddingCosts = euclideanEmbedding != null? euclideanEmbedding.costs.AsUnsafeSpanNoChecks() : default; + this.euclideanEmbeddingPivots = euclideanEmbedding != null ? (uint)euclideanEmbedding.pivotCount : 0; + this.targetNodeIndex = targetNodeIndex; + } + + public int Calculate (int3 point, uint nodeIndex) { + return Calculate(in this, ref point, nodeIndex); + } + + [BurstCompile] + public static int Calculate (in HeuristicObjective objective, ref int3 point, uint nodeIndex) { + var closest = math.clamp(point, objective.mn, objective.mx); + var diff = point - closest; + + int h; + switch (objective.heuristic) { + case Heuristic.Euclidean: + h = (int)(math.length((float3)diff) * objective.heuristicScale); + break; + case Heuristic.Manhattan: + h = (int)(math.csum(math.abs(diff)) * objective.heuristicScale); + break; + case Heuristic.DiagonalManhattan: + // Octile distance extended to 3D + diff = math.abs(diff); + var a = diff.x; + var b = diff.y; + var c = diff.z; + // Sort the values so that a <= b <= c + if (a > b) Memory.Swap(ref a, ref b); + if (b > c) Memory.Swap(ref b, ref c); + if (a > b) Memory.Swap(ref a, ref b); + + // This is the same as the Manhattan distance, but with a different weight for the diagonal moves. + const float SQRT_3 = 1.7321f; + const float SQRT_2 = 1.4142f; + h = (int)(objective.heuristicScale * (SQRT_3 * a + SQRT_2 * (b-a) + (c-b-a))); + break; + case Heuristic.None: + default: + h = 0; + break; + } + + if (objective.euclideanEmbeddingPivots > 0) { + h = math.max(h, (int)EuclideanEmbedding.GetHeuristic(objective.euclideanEmbeddingCosts, objective.euclideanEmbeddingPivots, nodeIndex, objective.targetNodeIndex)); + } + return h; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HeuristicObjective.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HeuristicObjective.cs.meta new file mode 100644 index 0000000..df7cd07 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HeuristicObjective.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f7f83de4e1bae824c801f70be70d1d5d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HierarchicalGraph.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HierarchicalGraph.cs new file mode 100644 index 0000000..73a580d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HierarchicalGraph.cs @@ -0,0 +1,604 @@ +// #define CHECK_INVARIANTS +using System.Collections.Generic; +using Pathfinding.Util; +using UnityEngine; +using Unity.Jobs; +using UnityEngine.Profiling; +using UnityEngine.Assertions; + +namespace Pathfinding { + using System.Runtime.InteropServices; + using Pathfinding.Drawing; + using Pathfinding.Jobs; + using Unity.Collections; + using Unity.Collections.LowLevel.Unsafe; + using Unity.Mathematics; + + /// <summary> + /// Holds a hierarchical graph to speed up certain pathfinding queries. + /// + /// A common type of query that needs to be very fast is on the form 'is this node reachable from this other node'. + /// This is for example used when picking the end node of a path. The end node is determined as the closest node to the end point + /// that can be reached from the start node. + /// + /// This data structure's primary purpose is to keep track of which connected component each node is contained in, in order to make such queries fast. + /// + /// See: https://en.wikipedia.org/wiki/Connected_component_(graph_theory) + /// + /// A connected component is a set of nodes such that there is a valid path between every pair of nodes in that set. + /// Thus the query above can simply be answered by checking if they are in the same connected component. + /// The connected component is exposed on nodes as the <see cref="Pathfinding.GraphNode.Area"/> property and on this class using the <see cref="GetConnectedComponent"/> method. + /// + /// Note: This class does not calculate strictly connected components. In case of one-way connections, it will still consider the nodes to be in the same connected component. + /// + /// In the image below (showing a 200x200 grid graph) each connected component is colored using a separate color. + /// The actual color doesn't signify anything in particular however, only that they are different. + /// [Open online documentation to see images] + /// + /// Prior to version 4.2 the connected components were just a number stored on each node, and when a graph was updated + /// the connected components were completely recalculated. This can be done relatively efficiently using a flood filling + /// algorithm (see https://en.wikipedia.org/wiki/Flood_fill) however it still requires a pass through every single node + /// which can be quite costly on larger graphs. + /// + /// This class instead builds a much smaller graph that still respects the same connectivity as the original graph. + /// Each node in this hierarchical graph represents a larger number of real nodes that are one single connected component. + /// Take a look at the image below for an example. In the image each color is a separate hierarchical node, and the black connections go between the center of each hierarchical node. + /// + /// [Open online documentation to see images] + /// + /// With the hierarchical graph, the connected components can be calculated by flood filling the hierarchical graph instead of the real graph. + /// Then when we need to know which connected component a node belongs to, we look up the connected component of the hierarchical node the node belongs to. + /// + /// The benefit is not immediately obvious. The above is just a bit more complicated way to accomplish the same thing. However the real benefit comes when updating the graph. + /// When the graph is updated, all hierarchical nodes which contain any node that was affected by the update is removed completely and then once all have been removed new hierarchical nodes are recalculated in their place. + /// Once this is done the connected components of the whole graph can be updated by flood filling only the hierarchical graph. Since the hierarchical graph is vastly smaller than the real graph, this is significantly faster. + /// + /// [Open online documentation to see videos] + /// + /// So finally using all of this, the connected components of the graph can be recalculated very quickly as the graph is updated. + /// The effect of this grows larger the larger the graph is, and the smaller the graph update is. Making a small update to a 1000x1000 grid graph is on the order of 40 times faster with these optimizations. + /// When scanning a graph or making updates to the whole graph at the same time there is however no speed boost. In fact due to the extra complexity it is a bit slower, however after profiling the extra time seems to be mostly insignificant compared to the rest of the cost of scanning the graph. + /// + /// [Open online documentation to see videos] + /// + /// See: <see cref="Pathfinding.PathUtilities.IsPathPossible"/> + /// See: <see cref="Pathfinding.NNConstraint"/> + /// See: <see cref="Pathfinding.GraphNode.Area"/> + /// </summary> + public class HierarchicalGraph { + const int Tiling = 16; + const int MaxChildrenPerNode = Tiling * Tiling; + const int MinChildrenPerNode = MaxChildrenPerNode/2; + + GlobalNodeStorage nodeStorage; + internal List<GraphNode>[] children; + internal NativeList<int> connectionAllocations; + internal SlabAllocator<int> connectionAllocator; + NativeList<int> dirtiedHierarchicalNodes; + int[] areas; + byte[] dirty; + int[] versions; + internal NativeList<Bounds> bounds; + /// <summary>Holds areas.Length as a burst-accessible reference</summary> + NativeReference<int> numHierarchicalNodes; + internal GCHandle gcHandle; + + public int version { get; private set; } + public NavmeshEdges navmeshEdges; + + Queue<GraphNode> temporaryQueue = new Queue<GraphNode>(); + List<int> currentConnections = new List<int>(); + Stack<int> temporaryStack = new Stack<int>(); + + HierarchicalBitset dirtyNodes; + + CircularBuffer<int> freeNodeIndices; + + int gizmoVersion = 0; + + RWLock rwLock = new RWLock(); + + /// <summary> + /// Disposes of all unmanaged data and clears managed data. + /// + /// If you want to use this instance again, you must call <see cref="OnEnable"/>. + /// </summary> + internal void OnDisable () { + rwLock.WriteSync().Unlock(); + navmeshEdges.Dispose(); + if (gcHandle.IsAllocated) gcHandle.Free(); + if (connectionAllocator.IsCreated) { + numHierarchicalNodes.Dispose(); + connectionAllocator.Dispose(); + connectionAllocations.Dispose(); + bounds.Dispose(); + dirtiedHierarchicalNodes.Dispose(); + dirtyNodes.Dispose(); + + children = null; + areas = null; + dirty = null; + versions = null; + freeNodeIndices.Clear(); + } + } + + // Make methods internal + public int GetHierarchicalNodeVersion (int index) { + return (index * 71237) ^ versions[index]; + } + + /// <summary>Burst-accessible data about the hierarhical nodes</summary> + public struct HierarhicalNodeData { + [Unity.Collections.ReadOnly] + public SlabAllocator<int> connectionAllocator; + [Unity.Collections.ReadOnly] + public NativeList<int> connectionAllocations; + [Unity.Collections.ReadOnly] + public NativeList<Bounds> bounds; + } + + /// <summary> + /// Data about the hierarhical nodes. + /// + /// Can be accessed in burst jobs. + /// </summary> + public HierarhicalNodeData GetHierarhicalNodeData (out RWLock.ReadLockAsync readLock) { + readLock = rwLock.Read(); + return new HierarhicalNodeData { + connectionAllocator = connectionAllocator, + connectionAllocations = connectionAllocations, + bounds = bounds, + }; + } + + internal HierarchicalGraph (GlobalNodeStorage nodeStorage) { + this.nodeStorage = nodeStorage; + navmeshEdges = new NavmeshEdges(); + navmeshEdges.hierarchicalGraph = this; + } + + /// <summary> + /// Initializes the HierarchicalGraph data. + /// It is safe to call this multiple times even if it has already been enabled before. + /// </summary> + public void OnEnable () { + if (!connectionAllocator.IsCreated) { + gcHandle = GCHandle.Alloc(this); + + connectionAllocator = new SlabAllocator<int>(1024, Allocator.Persistent); + connectionAllocations = new NativeList<int>(0, Allocator.Persistent); + bounds = new NativeList<Bounds>(0, Allocator.Persistent); + numHierarchicalNodes = new NativeReference<int>(0, Allocator.Persistent); + dirtiedHierarchicalNodes = new NativeList<int>(0, Allocator.Persistent); + dirtyNodes = new HierarchicalBitset(1024, Allocator.Persistent); + + children = new List<GraphNode>[1] { new List<GraphNode>() }; + areas = new int[1]; + dirty = new byte[1]; + versions = new int[1]; + freeNodeIndices.Clear(); + } + } + + internal void OnCreatedNode (GraphNode node) { + AddDirtyNode(node); + } + + internal void OnDestroyedNode (GraphNode node) { + dirty[node.HierarchicalNodeIndex] = 1; + dirtyNodes.Reset((int)node.NodeIndex); + node.IsHierarchicalNodeDirty = false; + } + + /// <summary> + /// Marks this node as dirty because it's connectivity or walkability has changed. + /// This must be called by node classes after any connectivity/walkability changes have been made to them. + /// + /// See: <see cref="GraphNode.SetConnectivityDirty"/> + /// </summary> + public void AddDirtyNode (GraphNode node) { + if (!node.IsHierarchicalNodeDirty) { + // We may be calling this when shutting down + if (!dirtyNodes.IsCreated || node.Destroyed) return; + + dirtyNodes.Set((int)node.NodeIndex); + + // Mark the associated hierarchical node as dirty to ensure it is recalculated or removed later. + // Nodes that have been unwalkable since the last update, will have HierarchicalNodeIndex=0, which is a dummy hierarchical node that is never used. + dirty[node.HierarchicalNodeIndex] = 1; + node.IsHierarchicalNodeDirty = true; + } + } + + public void ReserveNodeIndices (uint nodeIndexCount) { + dirtyNodes.Capacity = Mathf.Max(dirtyNodes.Capacity, (int)nodeIndexCount); + } + + public int NumConnectedComponents { get; private set; } + + /// <summary>Get the connected component index of a hierarchical node</summary> + public uint GetConnectedComponent (int hierarchicalNodeIndex) { + return (uint)areas[hierarchicalNodeIndex]; + } + + struct JobRecalculateComponents : IJob { + public System.Runtime.InteropServices.GCHandle hGraphGC; + public NativeList<int> connectionAllocations; + public NativeList<Bounds> bounds; + public NativeList<int> dirtiedHierarchicalNodes; + public NativeReference<int> numHierarchicalNodes; + + void Grow (HierarchicalGraph graph) { + var newChildren = new List<GraphNode>[System.Math.Max(64, graph.children.Length*2)]; + var newAreas = new int[newChildren.Length]; + var newDirty = new byte[newChildren.Length]; + var newVersions = new int[newChildren.Length]; + numHierarchicalNodes.Value = newChildren.Length; + + graph.children.CopyTo(newChildren, 0); + graph.areas.CopyTo(newAreas, 0); + graph.dirty.CopyTo(newDirty, 0); + graph.versions.CopyTo(newVersions, 0); + bounds.Resize(newChildren.Length, NativeArrayOptions.UninitializedMemory); + connectionAllocations.Resize(newChildren.Length, NativeArrayOptions.ClearMemory); + + // Create all necessary lists for the new nodes + // Iterate in reverse so that when popping from the freeNodeIndices + // stack we get numbers from smallest to largest (this is not + // necessary, but it makes the starting colors be a bit nicer when + // visualized in the scene view). + for (int i = newChildren.Length - 1; i >= graph.children.Length; i--) { + newChildren[i] = ListPool<GraphNode>.Claim(MaxChildrenPerNode); + connectionAllocations[i] = SlabAllocator<int>.InvalidAllocation; + if (i > 0) graph.freeNodeIndices.PushEnd(i); + } + connectionAllocations[0] = SlabAllocator<int>.InvalidAllocation; + + graph.children = newChildren; + graph.areas = newAreas; + graph.dirty = newDirty; + graph.versions = newVersions; + } + + int GetHierarchicalNodeIndex (HierarchicalGraph graph) { + if (graph.freeNodeIndices.Length == 0) Grow(graph); + return graph.freeNodeIndices.PopEnd(); + } + + void RemoveHierarchicalNode (HierarchicalGraph hGraph, int hierarchicalNode, bool removeAdjacentSmallNodes) { + Assert.AreNotEqual(hierarchicalNode, 0); + hGraph.freeNodeIndices.PushEnd(hierarchicalNode); + hGraph.versions[hierarchicalNode]++; + var connAllocation = connectionAllocations[hierarchicalNode]; + var conns = hGraph.connectionAllocator.GetSpan(connAllocation); + + for (int i = 0; i < conns.Length; i++) { + var adjacentHierarchicalNode = conns[i]; + // If dirty, this hierarchical node will be removed later anyway, so don't bother doing anything with it. + if (hGraph.dirty[adjacentHierarchicalNode] != 0) continue; + + if (removeAdjacentSmallNodes && hGraph.children[adjacentHierarchicalNode].Count < MinChildrenPerNode) { + hGraph.dirty[adjacentHierarchicalNode] = 2; + RemoveHierarchicalNode(hGraph, adjacentHierarchicalNode, false); + + // The connection list may have been reallocated, so we need to get it again + conns = hGraph.connectionAllocator.GetSpan(connAllocation); + } else { + // Remove the connection from the other node to this node as we are removing this node. + var otherConnections = hGraph.connectionAllocator.GetList(connectionAllocations[adjacentHierarchicalNode]); + otherConnections.Remove(hierarchicalNode); + // Update the allocation index of the list, in case it was reallocated + connectionAllocations[adjacentHierarchicalNode] = otherConnections.allocationIndex; + } + } + Assert.AreEqual(connectionAllocations[hierarchicalNode], connAllocation); + + hGraph.connectionAllocator.Free(connAllocation); + connectionAllocations[hierarchicalNode] = SlabAllocator<int>.InvalidAllocation; + + var nodeChildren = hGraph.children[hierarchicalNode]; + + // Ensure all children of dirty hierarchical nodes are included in the recalculation + var preDirty = hGraph.dirty[hierarchicalNode]; + for (int i = 0; i < nodeChildren.Count; i++) { + if (!nodeChildren[i].Destroyed) hGraph.AddDirtyNode(nodeChildren[i]); + } + // Put the dirty flag back to what it was before, as it might have been set to 1 by the AddDirtyNode call + hGraph.dirty[hierarchicalNode] = preDirty; + + nodeChildren.ClearFast(); + } + + [System.Diagnostics.Conditional("CHECK_INVARIANTS")] + void CheckConnectionInvariants () { + var hGraph = (HierarchicalGraph)hGraphGC.Target; + if (connectionAllocations.Length > 0) Assert.AreEqual(connectionAllocations[0], SlabAllocator<int>.InvalidAllocation); + for (int i = 0; i < connectionAllocations.Length; i++) { + if (connectionAllocations[i] != SlabAllocator<int>.InvalidAllocation) { + var conns = hGraph.connectionAllocator.GetSpan(connectionAllocations[i]); + for (int j = 0; j < conns.Length; j++) { + Assert.IsFalse(connectionAllocations[conns[j]] == SlabAllocator<int>.InvalidAllocation, "Invalid connection allocation"); + var otherConns = hGraph.connectionAllocator.GetSpan(connectionAllocations[conns[j]]); + if (!otherConns.Contains(i)) { + throw new System.Exception("Connections are not bidirectional"); + } + } + } + } + } + + [System.Diagnostics.Conditional("CHECK_INVARIANTS")] + void CheckPreUpdateInvariants () { + var hGraph = (HierarchicalGraph)hGraphGC.Target; + + if (connectionAllocations.Length > 0) Assert.AreEqual(connectionAllocations[0], SlabAllocator<int>.InvalidAllocation); + for (int i = 0; i < connectionAllocations.Length; i++) { + if (connectionAllocations[i] != SlabAllocator<int>.InvalidAllocation) { + var children = hGraph.children[i]; + for (int j = 0; j < children.Count; j++) { + if (!children[j].Destroyed) { + Assert.AreEqual(children[j].HierarchicalNodeIndex, i); + } + } + } + } + } + + [System.Diagnostics.Conditional("CHECK_INVARIANTS")] + void CheckChildInvariants () { + var hGraph = (HierarchicalGraph)hGraphGC.Target; + + if (connectionAllocations.Length > 0) Assert.AreEqual(connectionAllocations[0], SlabAllocator<int>.InvalidAllocation); + for (int i = 0; i < connectionAllocations.Length; i++) { + if (connectionAllocations[i] != SlabAllocator<int>.InvalidAllocation) { + var children = hGraph.children[i]; + for (int j = 0; j < children.Count; j++) { + Assert.IsFalse(children[j].Destroyed); + Assert.AreEqual(children[j].HierarchicalNodeIndex, i); + } + } + } + } + + struct Context { + public List<GraphNode> children; + public int hierarchicalNodeIndex; + public List<int> connections; + public uint graphindex; + public Queue<GraphNode> queue; + } + + + /// <summary>Run a BFS out from a start node and assign up to MaxChildrenPerNode nodes to the specified hierarchical node which are not already assigned to another hierarchical node</summary> + void FindHierarchicalNodeChildren (HierarchicalGraph hGraph, int hierarchicalNode, GraphNode startNode) { + Assert.AreNotEqual(hierarchicalNode, 0); + Assert.AreEqual(hGraph.children[hierarchicalNode].Count, 0); + hGraph.versions[hierarchicalNode]++; + + // We create a context and pass that by reference to the GetConnections method. + // This allows us to pass a non-capturing delegate, which does not require a heap allocation. + var queue = hGraph.temporaryQueue; + var context = new Context { + children = hGraph.children[hierarchicalNode], + hierarchicalNodeIndex = hierarchicalNode, + connections = hGraph.currentConnections, + graphindex = startNode.GraphIndex, + queue = queue, + }; + context.connections.Clear(); + context.children.Add(startNode); + context.queue.Enqueue(startNode); + startNode.HierarchicalNodeIndex = hierarchicalNode; + + GraphNode.GetConnectionsWithData<Context> visitConnection = (GraphNode neighbour, ref Context context) => { + if (neighbour.Destroyed) { + throw new System.InvalidOperationException("A node in a " + AstarPath.active.graphs[context.graphindex].GetType().Name + " contained a connection to a destroyed " + neighbour.GetType().Name + "."); + } + var hIndex = neighbour.HierarchicalNodeIndex; + if (hIndex == 0) { + if (context.children.Count < MaxChildrenPerNode && neighbour.Walkable && neighbour.GraphIndex == context.graphindex /* && (((GridNode)currentChildren[0]).XCoordinateInGrid/Tiling == ((GridNode)neighbour).XCoordinateInGrid/Tiling) && (((GridNode)currentChildren[0]).ZCoordinateInGrid/Tiling == ((GridNode)neighbour).ZCoordinateInGrid/Tiling)*/) { + neighbour.HierarchicalNodeIndex = context.hierarchicalNodeIndex; + context.queue.Enqueue(neighbour); + context.children.Add(neighbour); + } + } else if (hIndex != context.hierarchicalNodeIndex && !context.connections.Contains(hIndex)) { + // The Contains call can in theory be very slow as an hierarchical node may be adjacent to an arbitrary number of nodes. + // However in practice due to how the hierarchical nodes are constructed they will only be adjacent to a smallish (≈4-6) number of other nodes. + // So a Contains call will be much faster than say a Set lookup. + context.connections.Add(hIndex); + } + }; + + while (queue.Count > 0) queue.Dequeue().GetConnections(visitConnection, ref context, Connection.IncomingConnection | Connection.OutgoingConnection); + + for (int i = 0; i < hGraph.currentConnections.Count; i++) { + var otherHierarchicalNode = hGraph.currentConnections[i]; + Assert.AreNotEqual(otherHierarchicalNode, 0); + var otherAllocationIndex = connectionAllocations[otherHierarchicalNode]; + Assert.AreNotEqual(otherAllocationIndex, SlabAllocator<int>.InvalidAllocation); + var otherConnections = hGraph.connectionAllocator.GetList(otherAllocationIndex); + otherConnections.Add(hierarchicalNode); + // Update the allocation index in case the list was reallocated + connectionAllocations[otherHierarchicalNode] = otherConnections.allocationIndex; + } + + connectionAllocations[hierarchicalNode] = hGraph.connectionAllocator.Allocate(hGraph.currentConnections); + queue.Clear(); + } + + /// <summary>Flood fills the graph of hierarchical nodes and assigns the same area ID to all hierarchical nodes that are in the same connected component</summary> + void FloodFill (HierarchicalGraph hGraph) { + var areas = hGraph.areas; + for (int i = 0; i < areas.Length; i++) areas[i] = 0; + + Stack<int> stack = hGraph.temporaryStack; + int currentArea = 0; + for (int i = 1; i < areas.Length; i++) { + // Already taken care of, or does not exist + if (areas[i] != 0 || connectionAllocations[i] == SlabAllocator<int>.InvalidAllocation) continue; + + currentArea++; + areas[i] = currentArea; + stack.Push(i); + while (stack.Count > 0) { + int node = stack.Pop(); + var conns = hGraph.connectionAllocator.GetSpan(connectionAllocations[node]); + for (int j = conns.Length - 1; j >= 0; j--) { + var otherNode = conns[j]; + // Note: slightly important that this is != currentArea and not != 0 in case there are some connected, but not stongly connected components in the graph (this will happen in only veeery few types of games) + if (areas[otherNode] != currentArea) { + areas[otherNode] = currentArea; + stack.Push(otherNode); + } + } + } + } + + hGraph.NumConnectedComponents = System.Math.Max(1, currentArea + 1); + hGraph.version++; + } + + public void Execute () { + var hGraph = hGraphGC.Target as HierarchicalGraph; + CheckPreUpdateInvariants(); + Profiler.BeginSample("Recalculate Connected Components"); + var dirty = hGraph.dirty; + CheckConnectionInvariants(); + + Profiler.BeginSample("Remove"); + // Remove all hierarchical nodes and then build new hierarchical nodes in their place + // which take into account the new graph data. + var initialFreeLength = hGraph.freeNodeIndices.Length; + for (int i = 1; i < dirty.Length; i++) { + if (dirty[i] == 1) RemoveHierarchicalNode(hGraph, i, true); + } + + // Reset the dirty flag on all hierarchical nodes + for (int i = 1; i < dirty.Length; i++) dirty[i] = 0; + + // Reset the dirty flag on all nodes, and make sure they don't refer to their new destroyed hierarchical nodes + var buffer = new NativeArray<int>(512, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var nodeStorage = hGraph.nodeStorage; + foreach (var span in hGraph.dirtyNodes.GetIterator(buffer.AsUnsafeSpan())) { + for (int i = 0; i < span.Length; i++) { + var node = nodeStorage.GetNode((uint)span[i]); + node.IsHierarchicalNodeDirty = false; + node.HierarchicalNodeIndex = 0; + } + } + + Profiler.EndSample(); + CheckConnectionInvariants(); + + Profiler.BeginSample("Find"); + dirtiedHierarchicalNodes.Clear(); + foreach (var span in hGraph.dirtyNodes.GetIterator(buffer.AsUnsafeSpan())) { + for (int i = 0; i < span.Length; i++) { + var node = nodeStorage.GetNode((uint)span[i]); + + Assert.IsFalse(node.Destroyed); + if (!node.Destroyed && node.HierarchicalNodeIndex == 0 && node.Walkable) { + var hNode = GetHierarchicalNodeIndex(hGraph); + Profiler.BeginSample("FindChildren"); + FindHierarchicalNodeChildren(hGraph, hNode, node); + Profiler.EndSample(); + dirtiedHierarchicalNodes.Add(hNode); + } + } + } + + // These are hierarchical node indices that were pushed to the free id stack, and we did not immediately reuse them. + // This means that they have been destroyed, and we should notify the NavmeshEdges class about this. + for (int i = initialFreeLength; i < hGraph.freeNodeIndices.Length; i++) { + dirtiedHierarchicalNodes.Add(hGraph.freeNodeIndices[i]); + } + + hGraph.dirtyNodes.Clear(); + + // Recalculate the connected components of the hierarchical nodes + // This is usually very quick compared to the code above + FloodFill(hGraph); + Profiler.EndSample(); + hGraph.gizmoVersion++; + CheckConnectionInvariants(); + CheckChildInvariants(); + } + } + + /// <summary>Recalculate the hierarchical graph and the connected components if any nodes have been marked as dirty</summary> + public void RecalculateIfNecessary () { + // We need to complete both jobs here, because after this method + // the graph may change in arbitrary ways. The RecalculateObstacles job reads from the graph. + JobRecalculateIfNecessary().Complete(); + } + + /// <summary> + /// Schedule a job to recalculate the hierarchical graph and the connected components if any nodes have been marked as dirty. + /// Returns dependsOn if nothing has to be done. + /// + /// Note: Assumes the graph is unchanged until the returned dependency is completed. + /// </summary> + public JobHandle JobRecalculateIfNecessary (JobHandle dependsOn = default) { + OnEnable(); + if (!dirtyNodes.IsEmpty) { + var writeLock = rwLock.Write(); + var lastJob = new JobRecalculateComponents { + hGraphGC = gcHandle, + connectionAllocations = connectionAllocations, + bounds = bounds, + dirtiedHierarchicalNodes = dirtiedHierarchicalNodes, + numHierarchicalNodes = numHierarchicalNodes, + }.Schedule(JobHandle.CombineDependencies(writeLock.dependency, dependsOn)); + // We need to output both jobs as dependencies. + // Firstly they use some internal data (e.g. dirtiedHierarchicalNodes), so we need to set lastJob. + // Secondly, they read from the graph. And the graph data is only read-only until this returned dependency is completed. + lastJob = navmeshEdges.RecalculateObstacles(dirtiedHierarchicalNodes, numHierarchicalNodes, lastJob); + writeLock.UnlockAfter(lastJob); + return lastJob; + } else { + return dependsOn; + } + } + + /// <summary> + /// Recalculate everything from scratch. + /// This is primarily to be used for legacy code for compatibility reasons, not for any new code. + /// + /// See: <see cref="RecalculateIfNecessary"/> + /// </summary> + public void RecalculateAll () { + var writeLock = rwLock.WriteSync(); + AstarPath.active.data.GetNodes(AddDirtyNode); + writeLock.Unlock(); + RecalculateIfNecessary(); + } + + public void OnDrawGizmos (DrawingData gizmos, RedrawScope redrawScope) { + var hasher = new NodeHasher(AstarPath.active); + + hasher.Add(gizmoVersion); + + if (!gizmos.Draw(hasher, redrawScope)) { + var readLock = rwLock.ReadSync(); + try { + using (var builder = gizmos.GetBuilder(hasher, redrawScope)) { + for (int i = 0; i < areas.Length; i++) { + if (children[i].Count > 0) { + builder.WireBox(bounds[i].center, bounds[i].size); + var conns = connectionAllocator.GetSpan(connectionAllocations[i]); + for (int j = 0; j < conns.Length; j++) { + if (conns[j] > i) { + builder.Line(bounds[i].center, bounds[conns[j]].center, Color.black); + } + } + } + } + } + } finally { + readLock.Unlock(); + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HierarchicalGraph.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HierarchicalGraph.cs.meta new file mode 100644 index 0000000..e8db5b3 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/HierarchicalGraph.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: aa5d6e4a6adb04d4ca9b7e735addcbd5 +timeCreated: 1535050640 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/ITraversalProvider.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/ITraversalProvider.cs new file mode 100644 index 0000000..249077b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/ITraversalProvider.cs @@ -0,0 +1,105 @@ +namespace Pathfinding { + /// <summary> + /// Provides additional traversal information to a path request. + /// + /// Example implementation: + /// <code> + /// public class MyCustomTraversalProvider : ITraversalProvider { + /// public bool CanTraverse (Path path, GraphNode node) { + /// // Make sure that the node is walkable and that the 'enabledTags' bitmask + /// // includes the node's tag. + /// return node.Walkable && (path.enabledTags >> (int)node.Tag & 0x1) != 0; + /// // alternatively: + /// // return DefaultITraversalProvider.CanTraverse(path, node); + /// } + /// + /// /** [CanTraverseDefault] */ + /// public bool CanTraverse (Path path, GraphNode from, GraphNode to) { + /// return CanTraverse(path, to); + /// } + /// /** [CanTraverseDefault] */ + /// + /// public uint GetTraversalCost (Path path, GraphNode node) { + /// // The traversal cost is the sum of the penalty of the node's tag and the node's penalty + /// return path.GetTagPenalty((int)node.Tag) + node.Penalty; + /// // alternatively: + /// // return DefaultITraversalProvider.GetTraversalCost(path, node); + /// } + /// + /// // This can be omitted in Unity 2021.3 and newer because a default implementation (returning true) can be used there. + /// public bool filterDiagonalGridConnections { + /// get { + /// return true; + /// } + /// } + /// } + /// </code> + /// + /// See: traversal_provider (view in online documentation for working links) + /// </summary> + public interface ITraversalProvider { + /// <summary> + /// Filter diagonal connections using <see cref="GridGraph.cutCorners"/> for effects applied by this ITraversalProvider. + /// This includes tags and other effects that this ITraversalProvider controls. + /// + /// This only has an effect if <see cref="GridGraph.cutCorners"/> is set to false and your grid has <see cref="GridGraph.neighbours"/> set to Eight. + /// + /// Take this example, the grid is completely walkable, but an ITraversalProvider is used to make the nodes marked with '#' + /// as unwalkable. The agent 'S' is in the middle. + /// + /// <code> + /// .......... + /// ....#..... + /// ...<see cref="S"/>#.... + /// ....#..... + /// .......... + /// </code> + /// + /// If filterDiagonalGridConnections is false the agent will be free to use the diagonal connections to move away from that spot. + /// However, if filterDiagonalGridConnections is true (the default) then the diagonal connections will be disabled and the agent will be stuck. + /// + /// Typically, there are a few common use cases: + /// - If your ITraversalProvider makes walls and obstacles and you want it to behave identically to obstacles included in the original grid graph scan, then this should be true. + /// - If your ITraversalProvider is used for agent to agent avoidance and you want them to be able to move around each other more freely, then this should be false. + /// + /// See: <see cref="GridNode"/> + /// </summary> + bool filterDiagonalGridConnections => true; + + /// <summary>True if node should be able to be traversed by the path.</summary> + bool CanTraverse(Path path, GraphNode node) => DefaultITraversalProvider.CanTraverse(path, node); + + /// <summary> + /// True if the path can traverse a link between from and to and if to can be traversed itself. + /// If this method returns true then a call to CanTraverse(path,to) must also return true. + /// Thus this method is a more flexible version of <see cref="CanTraverse(Path,GraphNode)"/>. + /// + /// If you only need the functionality for <see cref="CanTraverse(Path,GraphNode)"/> then you may implement this method by just forwarding it to <see cref="CanTraverse(Path,GraphNode)"/> + /// + /// <code> + /// public bool CanTraverse (Path path, GraphNode from, GraphNode to) { + /// return CanTraverse(path, to); + /// } + /// </code> + /// </summary> + bool CanTraverse(Path path, GraphNode from, GraphNode to) => CanTraverse(path, to); + + /// <summary> + /// Cost of traversing a given node. + /// Should return the additional cost for traversing this node. By default if no tags or penalties + /// are used then the traversal cost is zero. A cost of 1000 corresponds roughly to the cost of moving 1 world unit. + /// </summary> + uint GetTraversalCost(Path path, GraphNode node) => DefaultITraversalProvider.GetTraversalCost(path, node); + } + + /// <summary>Convenience class to access the default implementation of the ITraversalProvider</summary> + public static class DefaultITraversalProvider { + public static bool CanTraverse (Path path, GraphNode node) { + return node.Walkable && (path == null || (path.enabledTags >> (int)node.Tag & 0x1) != 0); + } + + public static uint GetTraversalCost (Path path, GraphNode node) { + return node.Penalty + (path != null ? path.GetTagPenalty((int)node.Tag) : 0); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/ITraversalProvider.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/ITraversalProvider.cs.meta new file mode 100644 index 0000000..c85b200 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/ITraversalProvider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: fb11e2e7634f01646a8d62ad94c75291 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/Path.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/Path.cs new file mode 100644 index 0000000..51f5975 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/Path.cs @@ -0,0 +1,1095 @@ +//#define ASTAR_POOL_DEBUG //@SHOWINEDITOR Enables debugging of path pooling. Will log warnings and info messages about paths not beeing pooled correctly. + +using UnityEngine; +using System.Collections; +using System.Collections.Generic; +using Unity.Mathematics; + +namespace Pathfinding { + /// <summary>Base class for all path types</summary> + [Unity.Burst.BurstCompile] + public abstract class Path : IPathInternals { +#if ASTAR_POOL_DEBUG + private string pathTraceInfo = ""; + private List<string> claimInfo = new List<string>(); + ~Path() { + Debug.Log("Destroying " + GetType().Name + " instance"); + if (claimed.Count > 0) { + Debug.LogWarning("Pool Is Leaking. See list of claims:\n" + + "Each message below will list what objects are currently claiming the path." + + " These objects have removed their reference to the path object but has not called .Release on it (which is bad).\n" + pathTraceInfo+"\n"); + for (int i = 0; i < claimed.Count; i++) { + Debug.LogWarning("- Claim "+ (i+1) + " is by a " + claimed[i].GetType().Name + "\n"+claimInfo[i]); + } + } else { + Debug.Log("Some scripts are not using pooling.\n" + pathTraceInfo + "\n"); + } + } +#endif + + /// <summary>Data for the thread calculating this path</summary> + protected PathHandler pathHandler; + + /// <summary> + /// Callback to call when the path is complete. + /// This is usually sent to the Seeker component which post processes the path and then calls a callback to the script which requested the path + /// </summary> + public OnPathDelegate callback; + + /// <summary> + /// Immediate callback to call when the path is complete. + /// Warning: This may be called from a separate thread. Usually you do not want to use this one. + /// + /// See: callback + /// </summary> + public OnPathDelegate immediateCallback; + + /// <summary>Returns the state of the path in the pathfinding pipeline</summary> + public PathState PipelineState { get; private set; } + + /// <summary> + /// Provides additional traversal information to a path request. + /// See: traversal_provider (view in online documentation for working links) + /// </summary> + public ITraversalProvider traversalProvider; + + + /// <summary>Backing field for <see cref="CompleteState"/></summary> + protected PathCompleteState completeState; + + /// <summary> + /// Current state of the path. + /// Bug: This may currently be set to Complete before the path has actually been fully calculated. In particular the vectorPath and path lists may not have been fully constructed. + /// This can lead to race conditions when using multithreading. Try to avoid using this method to check for if the path is calculated right now, use <see cref="IsDone"/> instead. + /// </summary> + public PathCompleteState CompleteState { + get { return completeState; } + protected set { + // Locking is used to avoid multithreading race conditions in which, for example, + // the error state is set on the main thread to cancel the path, + // and then a pathfinding thread marks the path as completed, + // which would replace the error state (if a lock and check would not have been used). + // We lock on the path object itself. Users should rarely have to use the path object + // themselves for anything before the path is calculated, much less take a lock on it. + lock (this) { + // Once the path is put in the error state, it cannot be set to any other state + if (completeState != PathCompleteState.Error) completeState = value; + } + } + } + + /// <summary> + /// If the path failed, this is true. + /// See: <see cref="errorLog"/> + /// See: This is equivalent to checking path.CompleteState == PathCompleteState.Error + /// </summary> + public bool error { get { return CompleteState == PathCompleteState.Error; } } + + /// <summary> + /// Additional info on why a path failed. + /// See: <see cref="AstarPath.logPathResults"/> + /// </summary> + public string errorLog { get; private set; } + + /// <summary> + /// Holds the path as a <see cref="GraphNode"/> list. + /// + /// These are all nodes that the path traversed, as calculated by the pathfinding algorithm. + /// This may not be the same nodes as the post processed path traverses. + /// + /// See: <see cref="vectorPath"/> + /// </summary> + public List<GraphNode> path; + + /// <summary> + /// Holds the (possibly post-processed) path as a Vector3 list. + /// + /// This list may be modified by path modifiers to be smoother or simpler compared to the raw path generated by the pathfinding algorithm. + /// + /// See: modifiers (view in online documentation for working links) + /// See: <see cref="path"/> + /// </summary> + public List<Vector3> vectorPath; + + /// <summary>How long it took to calculate this path in milliseconds</summary> + public float duration; + + /// <summary>Number of nodes this path has searched</summary> + public int searchedNodes { get; protected set; } + + /// <summary> + /// True if the path is currently pooled. + /// Do not set this value. Only read. It is used internally. + /// + /// See: <see cref="PathPool"/> + /// </summary> + bool IPathInternals.Pooled { get; set; } + + /// <summary> + /// True if the Reset function has been called. + /// Used to alert users when they are doing something wrong. + /// </summary> + protected bool hasBeenReset; + + /// <summary>Constraint for how to search for nodes</summary> + public NNConstraint nnConstraint = PathNNConstraint.Walkable; + + /// <summary>Determines which heuristic to use</summary> + public Heuristic heuristic; + + /// <summary> + /// Scale of the heuristic values. + /// See: AstarPath.heuristicScale + /// </summary> + public float heuristicScale = 1F; + + /// <summary>ID of this path. Used to distinguish between different paths</summary> + public ushort pathID { get; private set; } + + /// <summary>Target to use for H score calculation.</summary> + protected GraphNode hTargetNode; + + /// <summary> + /// Target to use for H score calculations. + /// See: https://en.wikipedia.org/wiki/Admissible_heuristic + /// </summary> + protected HeuristicObjective heuristicObjective; + + internal ref HeuristicObjective heuristicObjectiveInternal => ref heuristicObjective; + + /// <summary> + /// Which graph tags are traversable. + /// This is a bitmask so -1 = all bits set = all tags traversable. + /// For example, to set bit 5 to true, you would do + /// <code> myPath.enabledTags |= 1 << 5; </code> + /// To set it to false, you would do + /// <code> myPath.enabledTags &= ~(1 << 5); </code> + /// + /// The Seeker has a popup field where you can set which tags to use. + /// Note: If you are using a Seeker. The Seeker will set this value to what is set in the inspector field on StartPath. + /// So you need to change the Seeker value via script, not set this value if you want to change it via script. + /// + /// See: <see cref="CanTraverse"/> + /// See: bitmasks (view in online documentation for working links) + /// </summary> + public int enabledTags = -1; + + /// <summary>List of zeroes to use as default tag penalties</summary> + internal static readonly int[] ZeroTagPenalties = new int[32]; + + /// <summary> + /// The tag penalties that are actually used. + /// See: <see cref="tagPenalties"/> + /// </summary> + protected int[] internalTagPenalties; + + /// <summary> + /// Penalties for each tag. + /// Tag 0 which is the default tag, will get a penalty of tagPenalties[0]. + /// These should only be non-negative values since the A* algorithm cannot handle negative penalties. + /// + /// When assigning an array to this property it must have a length of 32. + /// + /// Note: Setting this to null will make all tag penalties be treated as if they are zero. + /// + /// Note: If you are using a Seeker. The Seeker will set this value to what is set in the inspector field when you call seeker.StartPath. + /// So you need to change the Seeker's value via script, not set this value. + /// + /// See: <see cref="Seeker.tagPenalties"/> + /// </summary> + public int[] tagPenalties { + get { + return internalTagPenalties == ZeroTagPenalties ? null : internalTagPenalties; + } + set { + if (value == null) { + internalTagPenalties = ZeroTagPenalties; + } else { + if (value.Length != 32) throw new System.ArgumentException("tagPenalties must have a length of 32"); + + internalTagPenalties = value; + } + } + } + + /// <summary>Copies the given settings into this path</summary> + public void UseSettings (PathRequestSettings settings) { + nnConstraint.graphMask = settings.graphMask; + traversalProvider = settings.traversalProvider; + enabledTags = settings.traversableTags; + tagPenalties = settings.tagPenalties; + } + + /// <summary> + /// Total Length of the path. + /// Calculates the total length of the <see cref="vectorPath"/>. + /// Cache this rather than call this function every time since it will calculate the length every time, not just return a cached value. + /// Returns: Total length of <see cref="vectorPath"/>, if <see cref="vectorPath"/> is null positive infinity is returned. + /// </summary> + public float GetTotalLength () { + if (vectorPath == null) return float.PositiveInfinity; + float tot = 0; + for (int i = 0; i < vectorPath.Count-1; i++) tot += Vector3.Distance(vectorPath[i], vectorPath[i+1]); + return tot; + } + + /// <summary> + /// Waits until this path has been calculated and returned. + /// Allows for very easy scripting. + /// + /// <code> + /// IEnumerator Start () { + /// // Get the seeker component attached to this GameObject + /// var seeker = GetComponent<Seeker>(); + /// + /// var path = seeker.StartPath(transform.position, transform.position + Vector3.forward * 10, null); + /// // Wait... This may take a frame or two depending on how complex the path is + /// // The rest of the game will continue to run while we wait + /// yield return StartCoroutine(path.WaitForPath()); + /// // The path is calculated now + /// + /// // Draw the path in the scene view for 10 seconds + /// for (int i = 0; i < path.vectorPath.Count - 1; i++) { + /// Debug.DrawLine(path.vectorPath[i], path.vectorPath[i+1], Color.red, 10); + /// } + /// } + /// </code> + /// + /// Note: Do not confuse this with AstarPath.BlockUntilCalculated. This one will wait using yield until it has been calculated + /// while AstarPath.BlockUntilCalculated will halt all operations until the path has been calculated. + /// + /// Throws: System.InvalidOperationException if the path is not started. Send the path to <see cref="Seeker.StartPath(Path)"/> or <see cref="AstarPath.StartPath"/> before calling this function. + /// + /// See: <see cref="BlockUntilCalculated"/> + /// See: https://docs.unity3d.com/Manual/Coroutines.html + /// </summary> + public IEnumerator WaitForPath () { + if (PipelineState == PathState.Created) throw new System.InvalidOperationException("This path has not been started yet"); + + while (PipelineState != PathState.Returned) yield return null; + } + + /// <summary> + /// Blocks until this path has been calculated and returned. + /// Normally it takes a few frames for a path to be calculated and returned. + /// This function will ensure that the path will be calculated when this function returns + /// and that the callback for that path has been called. + /// + /// Use this function only if you really need to. + /// There is a point to spreading path calculations out over several frames. + /// It smoothes out the framerate and makes sure requesting a large + /// number of paths at the same time does not cause lag. + /// + /// Note: Graph updates and other callbacks might get called during the execution of this function. + /// + /// <code> + /// var path = seeker.StartPath(transform.position, transform.position + Vector3.forward * 10, OnPathComplete); + /// path.BlockUntilCalculated(); + /// + /// // The path is calculated now, and the OnPathComplete callback has been called + /// </code> + /// + /// See: This is equivalent to calling <see cref="AstarPath.BlockUntilCalculated(Path)"/> + /// See: <see cref="WaitForPath"/> + /// </summary> + public void BlockUntilCalculated () { + AstarPath.BlockUntilCalculated(this); + } + + /// <summary> + /// True if this path node might be worth exploring. + /// + /// This is used during a search to filter out nodes which have already been fully searched. + /// </summary> + public bool ShouldConsiderPathNode (uint pathNodeIndex) { + var node = pathHandler.pathNodes[pathNodeIndex]; + return node.pathID != pathID || node.heapIndex != BinaryHeap.NotInHeap; + } + + public static readonly Unity.Profiling.ProfilerMarker MarkerOpenCandidateConnectionsToEnd = new Unity.Profiling.ProfilerMarker("OpenCandidateConnectionsToEnd"); + public static readonly Unity.Profiling.ProfilerMarker MarkerTrace = new Unity.Profiling.ProfilerMarker("Trace"); + + /// <summary> + /// Open a connection to the temporary end node if necessary. + /// + /// The start and end nodes are temporary nodes and are not included in the graph itself. + /// This means that we need to handle connections to and from those nodes as a special case. + /// This function will open a connection from the given node to the end node, if such a connection exists. + /// + /// It is called from the <see cref="GraphNode.Open"/> function. + /// </summary> + /// <param name="position">Position of the path node that is being opened. This may be different from the node's position if \reflink{PathNode.fractionAlongEdge} is being used.</param> + /// <param name="parentPathNode">Index of the path node that is being opened. This is often the same as parentNodeIndex, but may be different if the node has multiple path node variants.</param> + /// <param name="parentNodeIndex">Index of the node that is being opened.</param> + /// <param name="parentG">G score of the parent node. The cost to reach the parent node from the start of the path.</param> + public void OpenCandidateConnectionsToEndNode (Int3 position, uint parentPathNode, uint parentNodeIndex, uint parentG) { + // True iff this node has a connection to one or more temporary nodes + if (pathHandler.pathNodes[parentNodeIndex].flag1) { + MarkerOpenCandidateConnectionsToEnd.Begin(); + for (uint i = 0; i < pathHandler.numTemporaryNodes; i++) { + var nodeIndex = pathHandler.temporaryNodeStartIndex + i; + ref var node = ref pathHandler.GetTemporaryNode(nodeIndex); + if (node.type == TemporaryNodeType.End && node.associatedNode == parentNodeIndex) { + var cost = (uint)(position - node.position).costMagnitude; + OpenCandidateConnection(parentPathNode, nodeIndex, parentG, cost, 0, node.position); + } + } + MarkerOpenCandidateConnectionsToEnd.End(); + } + } + + /// <summary> + /// Opens a connection between two nodes during the A* search. + /// + /// When a node is "opened" (i.e. searched by the A* algorithm), it will open connections to all its neighbours. + /// This function checks those connections to see if passing through the node to its neighbour is the best way to reach the neighbour that we have seen so far, + /// and if so, it will push the neighbour onto the search heap. + /// </summary> + /// <param name="parentPathNode">The node that is being opened.</param> + /// <param name="targetPathNode">A neighbour of the parent that is being considered.</param> + /// <param name="parentG">The G value of the parent node. This is the cost to reach the parent node from the start of the path.</param> + /// <param name="connectionCost">The cost of moving from the parent node to the target node.</param> + /// <param name="fractionAlongEdge">Internal value used by the TriangleMeshNode to store where on the shared edge between the nodes we say we cross over.</param> + /// <param name="targetNodePosition">The position of the target node. This is used by the heuristic to estimate the cost to reach the end node.</param> + public void OpenCandidateConnection (uint parentPathNode, uint targetPathNode, uint parentG, uint connectionCost, uint fractionAlongEdge, Int3 targetNodePosition) { + if (!ShouldConsiderPathNode(targetPathNode)) { + // We have seen this node before, but it is not in the heap. + // This means we have already processed it and it must have had a better F score than this node (or the heuristic was not admissable). + // We can safely discard this connection. + return; + } + + uint candidateEnteringCost; + uint targetNodeIndex; + if (pathHandler.IsTemporaryNode(targetPathNode)) { + candidateEnteringCost = 0; + targetNodeIndex = 0; + } else { + var targetNode = pathHandler.GetNode(targetPathNode); + candidateEnteringCost = GetTraversalCost(targetNode); + targetNodeIndex = targetNode.NodeIndex; + } + var candidateG = parentG + connectionCost + candidateEnteringCost; + var pars = new OpenCandidateParams { + pathID = pathID, + parentPathNode = parentPathNode, + targetPathNode = targetPathNode, + targetNodeIndex = targetNodeIndex, + candidateG = candidateG, + fractionAlongEdge = fractionAlongEdge, + targetNodePosition = (int3)targetNodePosition, + pathNodes = pathHandler.pathNodes, + }; + OpenCandidateConnectionBurst(ref pars, + ref pathHandler.heap, ref heuristicObjective + ); + } + + /// <summary> + /// Parameters to OpenCandidateConnectionBurst. + /// Using a struct instead of passing the parameters as separate arguments is significantly faster. + /// </summary> + public struct OpenCandidateParams { + public Util.UnsafeSpan<PathNode> pathNodes; + public uint parentPathNode; + public uint targetPathNode; + public uint targetNodeIndex; + public uint candidateG; + public uint fractionAlongEdge; + public int3 targetNodePosition; + public ushort pathID; + } + + /// <summary> + /// Burst-compiled internal implementation of OpenCandidateConnection. + /// Compiling it using burst provides a decent 25% speedup. + /// The function itself is much faster, but the overhead of calling it from C# is quite significant. + /// </summary> + [Unity.Burst.BurstCompile] + public static void OpenCandidateConnectionBurst (ref OpenCandidateParams pars, ref BinaryHeap heap, ref HeuristicObjective heuristicObjective) { + var pathID = pars.pathID; + var parentPathNode = pars.parentPathNode; + var targetPathNode = pars.targetPathNode; + var candidateG = pars.candidateG; + var fractionAlongEdge = pars.fractionAlongEdge; + var targetNodePosition = pars.targetNodePosition; + var pathNodes = pars.pathNodes; + ref var target = ref pathNodes[targetPathNode]; + if (target.pathID != pathID) { + // This is the first time we have seen this node. This connection must be optimal. + target.fractionAlongEdge = fractionAlongEdge; + target.pathID = pathID; + target.parentIndex = parentPathNode; + var candidateH = (uint)heuristicObjective.Calculate(targetNodePosition, pars.targetNodeIndex); + var candidateF = candidateG + candidateH; + heap.Add(pathNodes, targetPathNode, candidateG, candidateF); + } else { + // Note: Before this method is called, a check is done for the case target.pathID==pathID && heapIndex == NotInHeap, + // so we know target.heapIndex != NotInHeap here. + + // We have seen this node before and it is in the heap. + // Now we check if this path to the target node is better than the previous one. + + var targetG = heap.GetG(target.heapIndex); + // The previous F score of the node + var targetF = heap.GetF(target.heapIndex); + var targetH = targetF - targetG; + uint candidateH; + + if (target.fractionAlongEdge != fractionAlongEdge) { + // Different fractionAlongEdge, this means that targetNodePosition may have changed + // and therefore the heuristic may also have changed. + candidateH = (uint)heuristicObjective.Calculate(targetNodePosition, pars.targetNodeIndex); + } else { + // If fractionAlongEdge has not changed, then we assume the heuristic is also the same. + // This saves us from having to calculate it again. + candidateH = targetH; + } + + var candidateF = candidateG + candidateH; + if (candidateF < targetF) { + // This connection is better than the previous one. + target.fractionAlongEdge = fractionAlongEdge; + target.parentIndex = parentPathNode; + heap.Add(pathNodes, targetPathNode, candidateG, candidateF); + } else { + // This connection is not better than the previous one. + // We can safely discard this connection. + } + } + } + + /// <summary>Returns penalty for the given tag.</summary> + /// <param name="tag">A value between 0 (inclusive) and 32 (exclusive).</param> + public uint GetTagPenalty (int tag) { + return (uint)internalTagPenalties[tag]; + } + + /// <summary> + /// Returns if the node can be traversed. + /// This by default equals to if the node is walkable and if the node's tag is included in <see cref="enabledTags"/>. + /// + /// See: <see cref="traversalProvider"/> + /// </summary> + public bool CanTraverse (GraphNode node) { + // Use traversal provider if set, otherwise fall back on default behaviour + // This method is hot, but this branch is extremely well predicted so it + // doesn't affect performance much (profiling indicates it is just above + // the noise level, somewhere around 0%-0.3%) + if (traversalProvider != null) + return traversalProvider.CanTraverse(this, node); + + // Manually inlined code from DefaultITraversalProvider + unchecked { return node.Walkable && (enabledTags >> (int)node.Tag & 0x1) != 0; } + } + + + /// <summary> + /// Returns if the path can traverse a link between from and to and if to can be traversed itself. + /// This by default equals to if the to is walkable and if the to's tag is included in <see cref="enabledTags"/>. + /// + /// See: <see cref="traversalProvider"/> + /// </summary> + public bool CanTraverse (GraphNode from, GraphNode to) { + // Use traversal provider if set, otherwise fall back on default behaviour + // This method is hot, but this branch is extremely well predicted so it + // doesn't affect performance much (profiling indicates it is just above + // the noise level, somewhere around 0%-0.3%) + if (traversalProvider != null) + return traversalProvider.CanTraverse(this, from, to); + + // Manually inlined code from DefaultITraversalProvider + unchecked { return to.Walkable && (enabledTags >> (int)to.Tag & 0x1) != 0; } + } + + /// <summary>Returns the cost of traversing the given node</summary> + public uint GetTraversalCost (GraphNode node) { +#if ASTAR_NO_TRAVERSAL_COST + return 0; +#else + // Use traversal provider if set, otherwise fall back on default behaviour + if (traversalProvider != null) + return traversalProvider.GetTraversalCost(this, node); + + unchecked { return GetTagPenalty((int)node.Tag) + node.Penalty; } +#endif + } + + /// <summary> + /// True if this path is done calculating. + /// + /// Note: The callback for the path might not have been called yet. + /// + /// See: <see cref="Seeker.IsDone"/> which also takes into account if the path callback has been called and had modifiers applied. + /// </summary> + public bool IsDone () { + return PipelineState > PathState.Processing; + } + + /// <summary>Threadsafe increment of the state</summary> + void IPathInternals.AdvanceState (PathState s) { + lock (this) { + PipelineState = (PathState)System.Math.Max((int)PipelineState, (int)s); + } + } + + /// <summary>Causes the path to fail and sets <see cref="errorLog"/> to msg</summary> + public void FailWithError (string msg) { + Error(); + if (errorLog != "") errorLog += "\n" + msg; + else errorLog = msg; + } + + /// <summary> + /// Aborts the path because of an error. + /// Sets <see cref="error"/> to true. + /// This function is called when an error has occurred (e.g a valid path could not be found). + /// See: <see cref="FailWithError"/> + /// </summary> + public void Error () { + CompleteState = PathCompleteState.Error; + } + + /// <summary> + /// Performs some error checking. + /// Makes sure the user isn't using old code paths and that no major errors have been made. + /// + /// Causes the path to fail if any errors are found. + /// </summary> + private void ErrorCheck () { + if (!hasBeenReset) FailWithError("Please use the static Construct function for creating paths, do not use the normal constructors."); + if (((IPathInternals)this).Pooled) FailWithError("The path is currently in a path pool. Are you sending the path for calculation twice?"); + if (pathHandler == null) FailWithError("Field pathHandler is not set. Please report this bug."); + if (PipelineState > PathState.Processing) FailWithError("This path has already been processed. Do not request a path with the same path object twice."); + } + + /// <summary> + /// Called when the path enters the pool. + /// This method should release e.g pooled lists and other pooled resources + /// The base version of this method releases vectorPath and path lists. + /// Reset() will be called after this function, not before. + /// Warning: Do not call this function manually. + /// </summary> + protected virtual void OnEnterPool () { + if (vectorPath != null) Pathfinding.Util.ListPool<Vector3>.Release(ref vectorPath); + if (path != null) Pathfinding.Util.ListPool<GraphNode>.Release(ref path); + // Clear the callback to remove a potential memory leak + // while the path is in the pool (which it could be for a long time). + callback = null; + immediateCallback = null; + traversalProvider = null; + pathHandler = null; + } + + /// <summary> + /// Reset all values to their default values. + /// + /// Note: All inheriting path types (e.g ConstantPath, RandomPath, etc.) which declare their own variables need to + /// override this function, resetting ALL their variables to enable pooling of paths. + /// If this is not done, trying to use that path type for pooling could result in weird behaviour. + /// The best way is to reset to default values the variables declared in the extended path type and then + /// call the base function in inheriting types with base.Reset(). + /// </summary> + protected virtual void Reset () { +#if ASTAR_POOL_DEBUG + pathTraceInfo = "This path was got from the pool or created from here (stacktrace):\n"; + pathTraceInfo += System.Environment.StackTrace; +#endif + + if (System.Object.ReferenceEquals(AstarPath.active, null)) + throw new System.NullReferenceException("No AstarPath object found in the scene. " + + "Make sure there is one or do not create paths in Awake"); + + hasBeenReset = true; + PipelineState = (int)PathState.Created; + releasedNotSilent = false; + + pathHandler = null; + callback = null; + immediateCallback = null; + errorLog = ""; + completeState = PathCompleteState.NotCalculated; + + path = Pathfinding.Util.ListPool<GraphNode>.Claim(); + vectorPath = Pathfinding.Util.ListPool<Vector3>.Claim(); + + duration = 0; + searchedNodes = 0; + + nnConstraint = PathNNConstraint.Walkable; + + heuristic = AstarPath.active.heuristic; + heuristicScale = AstarPath.active.heuristicScale; + + enabledTags = -1; + tagPenalties = null; + + pathID = AstarPath.active.GetNextPathID(); + + hTargetNode = null; + + traversalProvider = null; + } + + /// <summary>List of claims on this path with reference objects</summary> + private List<System.Object> claimed = new List<System.Object>(); + + /// <summary> + /// True if the path has been released with a non-silent call yet. + /// + /// See: Release + /// See: Claim + /// </summary> + private bool releasedNotSilent; + + /// <summary> + /// Increase the reference count on this path by 1 (for pooling). + /// A claim on a path will ensure that it is not pooled. + /// If you are using a path, you will want to claim it when you first get it and then release it when you will not + /// use it anymore. When there are no claims on the path, it will be reset and put in a pool. + /// + /// This is essentially just reference counting. + /// + /// The object passed to this method is merely used as a way to more easily detect when pooling is not done correctly. + /// It can be any object, when used from a movement script you can just pass "this". This class will throw an exception + /// if you try to call Claim on the same path twice with the same object (which is usually not what you want) or + /// if you try to call Release with an object that has not been used in a Claim call for that path. + /// The object passed to the Claim method needs to be the same as the one you pass to this method. + /// + /// See: Release + /// See: Pool + /// See: pooling (view in online documentation for working links) + /// See: https://en.wikipedia.org/wiki/Reference_counting + /// </summary> + public void Claim (System.Object o) { + if (System.Object.ReferenceEquals(o, null)) throw new System.ArgumentNullException("o"); + + for (int i = 0; i < claimed.Count; i++) { + // Need to use ReferenceEquals because it might be called from another thread + if (System.Object.ReferenceEquals(claimed[i], o)) + throw new System.ArgumentException("You have already claimed the path with that object ("+o+"). Are you claiming the path with the same object twice?"); + } + + claimed.Add(o); +#if ASTAR_POOL_DEBUG + claimInfo.Add(o.ToString() + "\n\nClaimed from:\n" + System.Environment.StackTrace); +#endif + } + + /// <summary> + /// Reduces the reference count on the path by 1 (pooling). + /// Removes the claim on the path by the specified object. + /// When the reference count reaches zero, the path will be pooled, all variables will be cleared and the path will be put in a pool to be used again. + /// This is great for performance since fewer allocations are made. + /// + /// If the silent parameter is true, this method will remove the claim by the specified object + /// but the path will not be pooled if the claim count reches zero unless a non-silent Release call has been made earlier. + /// This is used by the internal pathfinding components such as Seeker and AstarPath so that they will not cause paths to be pooled. + /// This enables users to skip the claim/release calls if they want without the path being pooled by the Seeker or AstarPath and + /// thus causing strange bugs. + /// + /// See: Claim + /// See: PathPool + /// </summary> + public void Release (System.Object o, bool silent = false) { + if (o == null) throw new System.ArgumentNullException("o"); + + for (int i = 0; i < claimed.Count; i++) { + // Need to use ReferenceEquals because it might be called from another thread + if (System.Object.ReferenceEquals(claimed[i], o)) { + claimed.RemoveAt(i); +#if ASTAR_POOL_DEBUG + claimInfo.RemoveAt(i); +#endif + if (!silent) { + releasedNotSilent = true; + } + + if (claimed.Count == 0 && releasedNotSilent) { + PathPool.Pool(this); + } + return; + } + } + if (claimed.Count == 0) { + throw new System.ArgumentException("You are releasing a path which is not claimed at all (most likely it has been pooled already). " + + "Are you releasing the path with the same object ("+o+") twice?" + + "\nCheck out the documentation on path pooling for help."); + } + throw new System.ArgumentException("You are releasing a path which has not been claimed with this object ("+o+"). " + + "Are you releasing the path with the same object twice?\n" + + "Check out the documentation on path pooling for help."); + } + + /// <summary> + /// Traces the calculated path from the end node to the start. + /// This will build an array (<see cref="path)"/> of the nodes this path will pass through and also set the <see cref="vectorPath"/> array to the <see cref="path"/> arrays positions. + /// Assumes the <see cref="vectorPath"/> and <see cref="path"/> are empty and not null (which will be the case for a correctly initialized path). + /// </summary> + protected virtual void Trace (uint fromPathNodeIndex) { + MarkerTrace.Begin(); + // Current node we are processing + var c = fromPathNodeIndex; + int count = 0; + var pathNodes = pathHandler.pathNodes; + + while (c != 0) { + c = pathNodes[c].parentIndex; + count++; + if (count > 16384) { + Debug.LogWarning("Infinite loop? >16384 node path. Remove this message if you really have that long paths (Path.cs, Trace method)"); + break; + } + } + + // Ensure the lists have enough capacity + if (path.Capacity < count) path.Capacity = count; + UnityEngine.Assertions.Assert.AreEqual(0, path.Count); + + c = fromPathNodeIndex; + + GraphNode lastNode = null; + for (int i = 0; i < count; i++) { + GraphNode node; + if (pathHandler.IsTemporaryNode(c)) { + node = pathHandler.GetNode(pathHandler.GetTemporaryNode(c).associatedNode); + } else { + node = pathHandler.GetNode(c); + } + // If a node has multiple variants (like the triangle mesh node), then we may visit + // the same node multiple times in a sequence (but different variants of it). + // In the final path we don't want the duplicates. + if (node != lastNode) { + path.Add(node); + lastNode = node; + } + c = pathNodes[c].parentIndex; + } + + // Reverse + count = path.Count; + int half = count/2; + for (int i = 0; i < half; i++) { + var tmp = path[i]; + path[i] = path[count-i-1]; + path[count - i - 1] = tmp; + } + + if (vectorPath.Capacity < count) vectorPath.Capacity = count; + for (int i = 0; i < count; i++) { + vectorPath.Add((Vector3)path[i].position); + } + MarkerTrace.End(); + } + + /// <summary> + /// Writes text shared for all overrides of DebugString to the string builder. + /// See: DebugString + /// </summary> + protected void DebugStringPrefix (PathLog logMode, System.Text.StringBuilder text) { + text.Append(error ? "Path Failed : " : "Path Completed : "); + text.Append("Computation Time "); + text.Append(duration.ToString(logMode == PathLog.Heavy ? "0.000 ms " : "0.00 ms ")); + + text.Append("Searched Nodes ").Append(searchedNodes); + + if (!error) { + text.Append(" Path Length "); + text.Append(path == null ? "Null" : path.Count.ToString()); + } + } + + /// <summary> + /// Writes text shared for all overrides of DebugString to the string builder. + /// See: DebugString + /// </summary> + protected void DebugStringSuffix (PathLog logMode, System.Text.StringBuilder text) { + if (error) { + text.Append("\nError: ").Append(errorLog); + } + + // Can only print this from the Unity thread + // since otherwise an exception might be thrown + if (logMode == PathLog.Heavy && !AstarPath.active.IsUsingMultithreading) { + text.Append("\nCallback references "); + if (callback != null) text.Append(callback.Target.GetType().FullName).AppendLine(); + else text.AppendLine("NULL"); + } + + text.Append("\nPath Number ").Append(pathID).Append(" (unique id)"); + } + + /// <summary> + /// Returns a string with information about it. + /// More information is emitted when logMode == Heavy. + /// An empty string is returned if logMode == None + /// or logMode == OnlyErrors and this path did not fail. + /// </summary> + protected virtual string DebugString (PathLog logMode) { + if (logMode == PathLog.None || (!error && logMode == PathLog.OnlyErrors)) { + return ""; + } + + // Get a cached string builder for this thread + System.Text.StringBuilder text = pathHandler.DebugStringBuilder; + text.Length = 0; + + DebugStringPrefix(logMode, text); + DebugStringSuffix(logMode, text); + + return text.ToString(); + } + + /// <summary>Calls callback to return the calculated path. See: <see cref="callback"/></summary> + protected virtual void ReturnPath () { + if (callback != null) { + callback(this); + } + } + + /// <summary> + /// Prepares low level path variables for calculation. + /// Called before a path search will take place. + /// Always called before the Prepare, Initialize and CalculateStep functions + /// </summary> + protected void PrepareBase (PathHandler pathHandler) { + //Make sure the path has a reference to the pathHandler + this.pathHandler = pathHandler; + //Assign relevant path data to the pathHandler + pathHandler.InitializeForPath(this); + + // Make sure that internalTagPenalties is an array which has the length 32 + if (internalTagPenalties == null || internalTagPenalties.Length != 32) + internalTagPenalties = ZeroTagPenalties; + + try { + ErrorCheck(); + } catch (System.Exception e) { + FailWithError(e.Message); + } + } + + /// <summary> + /// Called before the path is started. + /// Called right before Initialize + /// </summary> + protected abstract void Prepare(); + + /// <summary> + /// Always called after the path has been calculated. + /// Guaranteed to be called before other paths have been calculated on + /// the same thread. + /// Use for cleaning up things like node tagging and similar. + /// </summary> + protected virtual void Cleanup () { + // Cleanup any flags set by temporary nodes + var pathNodes = pathHandler.pathNodes; + for (uint i = 0; i < pathHandler.numTemporaryNodes; i++) { + var nodeIndex = pathHandler.temporaryNodeStartIndex + i; + ref var node = ref pathHandler.GetTemporaryNode(nodeIndex); + + var associatedNode = pathHandler.GetNode(node.associatedNode); + for (uint v = 0; v < associatedNode.PathNodeVariants; v++) { + pathNodes[node.associatedNode + v].flag1 = false; + pathNodes[node.associatedNode + v].flag2 = false; + } + } + } + + protected int3 FirstTemporaryEndNode () { + for (uint i = 0; i < pathHandler.numTemporaryNodes; i++) { + var nodeIndex = pathHandler.temporaryNodeStartIndex + i; + ref var node = ref pathHandler.GetTemporaryNode(nodeIndex); + if (node.type == TemporaryNodeType.End) { + return (int3)node.position; + } + } + throw new System.InvalidOperationException("There are no end nodes in the path"); + } + + + protected void TemporaryEndNodesBoundingBox (out int3 mn, out int3 mx) { + // These represent a bounding box containing all valid end points. + // Typically there's only one end point, but in some cases there can be more. + mn = (int3)int.MaxValue; + mx = (int3)int.MinValue; + + for (uint i = 0; i < pathHandler.numTemporaryNodes; i++) { + var nodeIndex = pathHandler.temporaryNodeStartIndex + i; + ref var node = ref pathHandler.GetTemporaryNode(nodeIndex); + if (node.type == TemporaryNodeType.End) { + mn = math.min(mn, (int3)node.position); + mx = math.max(mx, (int3)node.position); + } + } + } + + protected void MarkNodesAdjacentToTemporaryEndNodes () { + var pathNodes = pathHandler.pathNodes; + + for (uint i = 0; i < pathHandler.numTemporaryNodes; i++) { + var nodeIndex = pathHandler.temporaryNodeStartIndex + i; + ref var node = ref pathHandler.GetTemporaryNode(nodeIndex); + if (node.type == TemporaryNodeType.End) { + // Mark node with flag1 to mark it as a node connected to an end node + var associatedNode = pathHandler.GetNode(node.associatedNode); + for (uint v = 0; v < associatedNode.PathNodeVariants; v++) { + pathNodes[node.associatedNode + v].flag1 = true; + } + } + } + } + + protected void AddStartNodesToHeap () { + var pathNodes = pathHandler.pathNodes; + for (uint i = 0; i < pathHandler.numTemporaryNodes; i++) { + var nodeIndex = pathHandler.temporaryNodeStartIndex + i; + ref var node = ref pathHandler.GetTemporaryNode(nodeIndex); + if (node.type == TemporaryNodeType.Start) { + // Note: Setting F score to 0 is technically incorrect, but it doesn't + // matter since we will open the start nodes first anyway. + pathHandler.heap.Add(pathNodes, nodeIndex, 0, 0); + } + } + } + + /// <summary> + /// Called when there are no more nodes to search. + /// + /// This may be used to calculate a partial path as a fallback. + /// </summary> + protected abstract void OnHeapExhausted(); + + /// <summary> + /// Called when a valid node has been found for the end of the path. + /// + /// This function should trace the path back to the start node, and set CompleteState to Complete. + /// If CompleteState is unchanged, the search will continue. + /// </summary> + protected abstract void OnFoundEndNode(uint pathNode, uint hScore, uint gScore); + + /// <summary> + /// Called for every node that the path visits. + /// + /// This is used by path types to check if the target node has been reached, to log debug data, etc. + /// </summary> + public virtual void OnVisitNode (uint pathNode, uint hScore, uint gScore) {} + + /// <summary> + /// Calculates the path until completed or until the time has passed targetTick. + /// Usually a check is only done every 500 nodes if the time has passed targetTick. + /// Time/Ticks are got from System.DateTime.UtcNow.Ticks. + /// + /// Basic outline of what the function does for the standard path (Pathfinding.ABPath). + /// <code> + /// while the end has not been found and no error has occurred + /// pop the next node of the heap and set it as current + /// check if we have reached the end + /// if so, exit and return the path + /// + /// open the current node, i.e loop through its neighbours, mark them as visited and put them on a heap + /// + /// check if there are still nodes left to process (or have we searched the whole graph) + /// if there are none, flag error and exit + /// + /// check if the function has exceeded the time limit + /// if so, return and wait for the function to get called again + /// </code> + /// </summary> + protected virtual void CalculateStep (long targetTick) { + int counter = 0; + var pathNodes = pathHandler.pathNodes; + var temporaryNodeStartIndex = pathHandler.temporaryNodeStartIndex; + + // Continue to search as long as we haven't encountered an error and we haven't found the target + while (CompleteState == PathCompleteState.NotCalculated) { + searchedNodes++; + + // Any nodes left to search? + if (pathHandler.heap.isEmpty) { + OnHeapExhausted(); + return; + } + + // Select the node with the lowest F score and remove it from the open list + var currentPathNodeIndex = pathHandler.heap.Remove(pathNodes, out uint currentNodeG, out uint currentNodeF); + var currentNodeH = currentNodeF - currentNodeG; + + if (currentPathNodeIndex >= temporaryNodeStartIndex) { + // This is a special node + var node = pathHandler.GetTemporaryNode(currentPathNodeIndex); + if (node.type == TemporaryNodeType.Start) { + // A start node. We should open the associated node at this point + pathHandler.GetNode(node.associatedNode).OpenAtPoint(this, currentPathNodeIndex, node.position, currentNodeG); + } else if (node.type == TemporaryNodeType.End) { + // An end node. Yay! We found the path we wanted. + // Now we can just trace the path back to the start and return that. + // However, some path types may choose to continue the search to find more end points (e.g. the multi target path). + { + // Make sure we visit the node associated with the end node. + // This is usually redundant, but it can matter in some cases. + // In particular, triangle mesh nodes can be opened in such a way that the temporary end node + // gets a lower F score than the individual sides of the triangle. This means that the temporary end + // node will be searched before the triangle sides are searched and that might complete the path. + // This would lead to us never actually calling LogVisitedNode for the triangle node, if we didn't have this code. + pathHandler.LogVisitedNode(node.associatedNode, currentNodeH, currentNodeG); + } + OnFoundEndNode(currentPathNodeIndex, currentNodeH, currentNodeG); + if (CompleteState == PathCompleteState.Complete) { + return; + } + } + } else { + pathHandler.LogVisitedNode(currentPathNodeIndex, currentNodeH, currentNodeG); + + OnVisitNode(currentPathNodeIndex, currentNodeH, currentNodeG); + + // Loop through all walkable neighbours of the node and add them to the open list. + var node = pathHandler.GetNode(currentPathNodeIndex); + node.Open(this, currentPathNodeIndex, currentNodeG); + } + + // Check for time every 500 nodes, roughly every 0.5 ms usually + if (counter > 500) { + // Have we exceded the maxFrameTime, if so we should wait one frame before continuing the search since we don't want the game to lag + if (System.DateTime.UtcNow.Ticks >= targetTick) { + return; + } + counter = 0; + + // Mostly for development + if (searchedNodes > 1000000) { + throw new System.Exception("Probable infinite loop. Over 1,000,000 nodes searched"); + } + } + + counter++; + } + } + + PathHandler IPathInternals.PathHandler { get { return pathHandler; } } + void IPathInternals.OnEnterPool () { OnEnterPool(); } + void IPathInternals.Reset () { Reset(); } + void IPathInternals.ReturnPath () { ReturnPath(); } + void IPathInternals.PrepareBase (PathHandler handler) { PrepareBase(handler); } + void IPathInternals.Prepare () { Prepare(); } + void IPathInternals.Cleanup () { Cleanup(); } + void IPathInternals.CalculateStep (long targetTick) { CalculateStep(targetTick); } + string IPathInternals.DebugString (PathLog logMode) { return DebugString(logMode); } + } + + /// <summary>Used for hiding internal methods of the Path class</summary> + internal interface IPathInternals { + PathHandler PathHandler { get; } + bool Pooled { get; set; } + void AdvanceState(PathState s); + void OnEnterPool(); + void Reset(); + void ReturnPath(); + void PrepareBase(PathHandler handler); + void Prepare(); + void Cleanup(); + void CalculateStep(long targetTick); + string DebugString(PathLog logMode); + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/Path.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/Path.cs.meta new file mode 100644 index 0000000..17cffcf --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/Path.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: b179046d83ec84f0c91efc5335f78a30 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathHandler.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathHandler.cs new file mode 100644 index 0000000..debafc1 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathHandler.cs @@ -0,0 +1,249 @@ +using System.Collections.Generic; +using Unity.Collections; +using UnityEngine; + +namespace Pathfinding { + using Pathfinding.Util; + + /// <summary> + /// Stores temporary node data for a single pathfinding request. + /// Every node has one PathNode per thread used. + /// It stores e.g G score, H score and other temporary variables needed + /// for path calculation, but which are not part of the graph structure. + /// + /// See: Pathfinding.PathHandler + /// See: https://en.wikipedia.org/wiki/A*_search_algorithm + /// </summary> + public struct PathNode { + /// <summary>The path request (in this thread, if multithreading is used) which last used this node</summary> + public ushort pathID; + + /// <summary> + /// Index of the node in the binary heap. + /// The open list in the A* algorithm is backed by a binary heap. + /// To support fast 'decrease key' operations, the index of the node + /// is saved here. + /// </summary> + public ushort heapIndex; + + /// <summary>Bitpacked variable which stores several fields</summary> + private uint flags; + + public static readonly PathNode Default = new PathNode { pathID = 0, heapIndex = BinaryHeap.NotInHeap, flags = 0 }; + + /// <summary>Parent index uses the first 26 bits</summary> + private const uint ParentIndexMask = (1U << 26) - 1U; + + private const int FractionAlongEdgeOffset = 26; + private const uint FractionAlongEdgeMask = ((1U << 30) - 1U) & ~ParentIndexMask; + public const int FractionAlongEdgeQuantization = 1 << (30 - 26); + + public static uint ReverseFractionAlongEdge(uint v) => (FractionAlongEdgeQuantization - 1) - v; + + public static uint QuantizeFractionAlongEdge (float v) { + v *= FractionAlongEdgeQuantization - 1; + v += 0.5f; + return Unity.Mathematics.math.clamp((uint)v, 0, FractionAlongEdgeQuantization - 1); + } + + public static float UnQuantizeFractionAlongEdge (uint v) { + return (float)v * (1.0f / (FractionAlongEdgeQuantization - 1)); + } + + /// <summary>Flag 1 is at bit 30</summary> + private const int Flag1Offset = 30; + private const uint Flag1Mask = 1U << Flag1Offset; + + /// <summary>Flag 2 is at bit 31</summary> + private const int Flag2Offset = 31; + private const uint Flag2Mask = 1U << Flag2Offset; + + public uint fractionAlongEdge { + get => (flags & FractionAlongEdgeMask) >> FractionAlongEdgeOffset; + set => flags = (flags & ~FractionAlongEdgeMask) | ((value << FractionAlongEdgeOffset) & FractionAlongEdgeMask); + } + + public uint parentIndex { + get => flags & ParentIndexMask; + set => flags = (flags & ~ParentIndexMask) | value; + } + + /// <summary> + /// Use as temporary flag during pathfinding. + /// Path types can use this during pathfinding to mark + /// nodes. When done, this flag should be reverted to its default state (false) to + /// avoid messing up other pathfinding requests. + /// </summary> + public bool flag1 { + get => (flags & Flag1Mask) != 0; + set => flags = (flags & ~Flag1Mask) | (value ? Flag1Mask : 0U); + } + + /// <summary> + /// Use as temporary flag during pathfinding. + /// Path types can use this during pathfinding to mark + /// nodes. When done, this flag should be reverted to its default state (false) to + /// avoid messing up other pathfinding requests. + /// </summary> + public bool flag2 { + get => (flags & Flag2Mask) != 0; + set => flags = (flags & ~Flag2Mask) | (value ? Flag2Mask : 0U); + } + } + + public enum TemporaryNodeType { + Start, + End, + Ignore, + } + + public struct TemporaryNode { + public uint associatedNode; + public Int3 position; + public int targetIndex; + public TemporaryNodeType type; + } + + /// <summary>Handles thread specific path data.</summary> + public class PathHandler { + /// <summary> + /// Current PathID. + /// See: <see cref="PathID"/> + /// </summary> + private ushort pathID; + + public readonly int threadID; + public readonly int totalThreadCount; + internal readonly GlobalNodeStorage nodeStorage; + public int numTemporaryNodes { get; private set; } + + /// <summary> + /// All path nodes with an index greater or equal to this are temporary nodes that only exist for the duration of a single path. + /// + /// This is a copy of NodeStorage.nextNodeIndex. This is used to avoid having to access the NodeStorage while pathfinding as it's an extra indirection. + /// </summary> + public uint temporaryNodeStartIndex { get; private set; } + UnsafeSpan<TemporaryNode> temporaryNodes; + + /// <summary> + /// Reference to the per-node data for this thread. + /// + /// Note: Only guaranteed to point to a valid allocation while the path is being calculated. + /// </summary> + public UnsafeSpan<PathNode> pathNodes; +#if UNITY_EDITOR + UnsafeSpan<GlobalNodeStorage.DebugPathNode> debugPathNodes; +#endif + + /// <summary> + /// Binary heap to keep track of nodes on the "Open list". + /// See: https://en.wikipedia.org/wiki/A*_search_algorithm + /// </summary> + public BinaryHeap heap = new BinaryHeap(128); + + /// <summary>ID for the path currently being calculated or last path that was calculated</summary> + public ushort PathID { get { return pathID; } } + + /// <summary> + /// StringBuilder that paths can use to build debug strings. + /// Better for performance and memory usage to use a single StringBuilder instead of each path creating its own + /// </summary> + public readonly System.Text.StringBuilder DebugStringBuilder = new System.Text.StringBuilder(); + + internal PathHandler (GlobalNodeStorage nodeStorage, int threadID, int totalThreadCount) { + this.threadID = threadID; + this.totalThreadCount = totalThreadCount; + this.nodeStorage = nodeStorage; + temporaryNodes = new UnsafeSpan<TemporaryNode>(Allocator.Persistent, GlobalNodeStorage.MaxTemporaryNodes); + } + + public void InitializeForPath (Path p) { + var lastPathId = pathID; + pathID = p.pathID; + numTemporaryNodes = 0; + temporaryNodeStartIndex = nodeStorage.nextNodeIndex; + // Get the path nodes for this thread (may have been resized since last we calculated a path) + pathNodes = nodeStorage.pathfindingThreadData[threadID].pathNodes; + +#if UNITY_EDITOR + var astar = AstarPath.active; + var shouldLog = astar.showGraphs && (astar.debugMode == GraphDebugMode.F || astar.debugMode == GraphDebugMode.H || astar.debugMode == GraphDebugMode.G || astar.showSearchTree); + debugPathNodes = shouldLog ? nodeStorage.pathfindingThreadData[threadID].debugPathNodes : default; +#endif + + // Path IDs have overflowed 65K, cleanup is needed to avoid bugs where we think + // we have already visited a node when we haven't. + // Since pathIDs are handed out sequentially, we can check if the new path id + // is smaller than the last one. + if (pathID < lastPathId) { + ClearPathIDs(); + } + } + + /// <summary> + /// Returns the PathNode corresponding to the specified node. + /// The PathNode is specific to this PathHandler since multiple PathHandlers + /// are used at the same time if multithreading is enabled. + /// </summary> + public ref PathNode GetPathNode (GraphNode node, uint variant = 0) { + return ref pathNodes[node.NodeIndex + variant]; + } + + public bool IsTemporaryNode(uint pathNodeIndex) => pathNodeIndex >= temporaryNodeStartIndex; + + public uint AddTemporaryNode (TemporaryNode node) { + if (numTemporaryNodes >= GlobalNodeStorage.MaxTemporaryNodes) { + // It would be nice if we could dynamically re-allocate the temporaryNodes array, and the pathNodes array. But this class allows handing out references to path nodes and temporary nodes, + // and we cannot guarantee that those references will not be used after this function is called (which may lead to memory corruption). + // So instead we just have a hard limit, which can be increased by enabling the ASTAR_MORE_MULTI_TARGET_PATH_TARGETS define. + throw new System.InvalidOperationException("Cannot create more than " + GlobalNodeStorage.MaxTemporaryNodes + " temporary nodes. You can enable ASTAR_MORE_MULTI_TARGET_PATH_TARGETS in the A* Inspector optimizations tab to increase this limit."); + } + + var index = temporaryNodeStartIndex + (uint)numTemporaryNodes; + temporaryNodes[numTemporaryNodes] = node; + pathNodes[index] = PathNode.Default; + numTemporaryNodes++; + return index; + } + + public GraphNode GetNode(uint nodeIndex) => nodeStorage.GetNode(nodeIndex); + + public ref TemporaryNode GetTemporaryNode (uint nodeIndex) { + if (nodeIndex < temporaryNodeStartIndex || nodeIndex >= temporaryNodeStartIndex + numTemporaryNodes) + throw new System.ArgumentOutOfRangeException(); + return ref temporaryNodes[(int)(nodeIndex - temporaryNodeStartIndex)]; + } + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public void LogVisitedNode (uint pathNodeIndex, uint h, uint g) { +#if UNITY_EDITOR + if (debugPathNodes.Length > 0 && !IsTemporaryNode(pathNodeIndex)) { + var parent = pathNodes[pathNodeIndex].parentIndex; + debugPathNodes[pathNodeIndex] = new GlobalNodeStorage.DebugPathNode { + h = h, + g = g, + parentIndex = parent >= temporaryNodeStartIndex ? 0 : parent, + pathID = pathID, + fractionAlongEdge = (byte)pathNodes[pathNodeIndex].fractionAlongEdge, + }; + } +#endif + } + + /// <summary> + /// Set all nodes' pathIDs to 0. + /// See: Pathfinding.PathNode.pathID + /// </summary> + public void ClearPathIDs () { + for (int i = 0; i < pathNodes.Length; i++) { + pathNodes[i].pathID = 0; + } + } + + public void Dispose () { + heap.Dispose(); + temporaryNodes.Free(Allocator.Persistent); + pathNodes = default; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathHandler.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathHandler.cs.meta new file mode 100644 index 0000000..b197ba3 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathHandler.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 7cc885607b0534cc6a4a34c4caa58d89 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathProcessor.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathProcessor.cs new file mode 100644 index 0000000..b41a159 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathProcessor.cs @@ -0,0 +1,520 @@ +using UnityEngine; +using System.Collections; +using System.Collections.Generic; +using System.Threading; +using UnityEngine.Profiling; +using UnityEngine.Assertions; + +namespace Pathfinding { +#if NETFX_CORE + using Thread = Pathfinding.WindowsStore.Thread; +#else + using Thread = System.Threading.Thread; +#endif + + public class PathProcessor { + public event System.Action<Path> OnPathPreSearch; + public event System.Action<Path> OnPathPostSearch; + public event System.Action OnQueueUnblocked; + + internal BlockableChannel<Path> queue; + readonly AstarPath astar; + readonly PathReturnQueue returnQueue; + + PathHandler[] pathHandlers; + + /// <summary>References to each of the pathfinding threads</summary> + Thread[] threads; + bool multithreaded; + + /// <summary> + /// When no multithreading is used, the IEnumerator is stored here. + /// When no multithreading is used, a coroutine is used instead. It is not directly called with StartCoroutine + /// but a separate function has just a while loop which increments the main IEnumerator. + /// This is done so other functions can step the thread forward at any time, without having to wait for Unity to update it. + /// See: <see cref="CalculatePaths"/> + /// See: <see cref="CalculatePathsThreaded"/> + /// </summary> + IEnumerator threadCoroutine; + BlockableChannel<Path>.Receiver coroutineReceiver; + + readonly List<int> locks = new List<int>(); + int nextLockID = 0; + + static readonly Unity.Profiling.ProfilerMarker MarkerCalculatePath = new Unity.Profiling.ProfilerMarker("Calculating Path"); + static readonly Unity.Profiling.ProfilerMarker MarkerPreparePath = new Unity.Profiling.ProfilerMarker("Prepare Path"); + + /// <summary> + /// Number of parallel pathfinders. + /// Returns the number of concurrent processes which can calculate paths at once. + /// When using multithreading, this will be the number of threads, if not using multithreading it is always 1 (since only 1 coroutine is used). + /// See: threadInfos + /// See: IsUsingMultithreading + /// </summary> + public int NumThreads { + get { + return pathHandlers.Length; + } + } + + /// <summary>Returns whether or not multithreading is used</summary> + public bool IsUsingMultithreading { + get { + return multithreaded; + } + } + + internal PathProcessor (AstarPath astar, PathReturnQueue returnQueue, int processors, bool multithreaded) { + this.astar = astar; + this.returnQueue = returnQueue; + + // Set up path queue with the specified number of receivers + queue = new BlockableChannel<Path>(); + threads = null; + threadCoroutine = null; + pathHandlers = new PathHandler[0]; + } + + /// <summary> + /// Changes the number of threads used for pathfinding. + /// + /// If multithreading is disabled, processors must be equal to 1. + /// </summary> + public void SetThreadCount (int processors, bool multithreaded) { + if (threads != null || threadCoroutine != null || pathHandlers.Length > 0) throw new System.Exception("Call StopThreads before setting the thread count"); + + if (processors < 1) { + throw new System.ArgumentOutOfRangeException("processors"); + } + + if (!multithreaded && processors != 1) { + throw new System.Exception("Only a single non-multithreaded processor is allowed"); + } + + pathHandlers = new PathHandler[processors]; + this.multithreaded = multithreaded; + + for (int i = 0; i < processors; i++) { + pathHandlers[i] = new PathHandler(astar.nodeStorage, i, processors); + } + astar.nodeStorage.SetThreadCount(processors); + StartThreads(); + } + + void StartThreads () { + if (threads != null || threadCoroutine != null) throw new System.Exception("Call StopThreads before starting threads"); + + queue.Reopen(); + + // Ensure the node storage is up to date. + // Per-thread data may have been cleared if the AstarPath object + // was disabled. + astar.nodeStorage.SetThreadCount(pathHandlers.Length); + + if (multithreaded) { + threads = new Thread[this.pathHandlers.Length]; + + // Start lots of threads + for (int i = 0; i < this.pathHandlers.Length; i++) { + var pathHandler = pathHandlers[i]; + var receiver = queue.AddReceiver(); + threads[i] = new Thread(() => CalculatePathsThreaded(pathHandler, receiver)); +#if !UNITY_SWITCH || UNITY_EDITOR + // Note: Setting the thread name seems to crash when deploying for Switch: https://forum.arongranberg.com/t/path-processor-crashing-nintendo-switch-build/6584 + threads[i].Name = "Pathfinding Thread " + i; +#endif + threads[i].IsBackground = true; + threads[i].Start(); + } + } else { + coroutineReceiver = queue.AddReceiver(); + // Start coroutine if not using multithreading + threadCoroutine = CalculatePaths(pathHandlers[0]); + } + } + + /// <summary>Prevents pathfinding from running while held</summary> + public struct GraphUpdateLock { + PathProcessor pathProcessor; + int id; + + public GraphUpdateLock (PathProcessor pathProcessor, bool block) { + this.pathProcessor = pathProcessor; + Profiler.BeginSample("Pausing pathfinding"); + id = pathProcessor.Lock(block); + Profiler.EndSample(); + } + + /// <summary> + /// True while this lock is preventing the pathfinding threads from processing more paths. + /// Note that the pathfinding threads may not be paused yet (if this lock was obtained using PausePathfinding(false)). + /// </summary> + public bool Held => pathProcessor != null && pathProcessor.locks.Contains(id); + + /// <summary>Allow pathfinding to start running again if no other locks are still held</summary> + public void Release() => pathProcessor.Unlock(id); + } + + int Lock (bool block) { + queue.isBlocked = true; + if (block) { + while (!queue.allReceiversBlocked) { + Assert.IsTrue(threads != null || threadCoroutine != null); + if (IsUsingMultithreading) { + Thread.Sleep(1); + } else { + TickNonMultithreaded(); + } + } + } + + nextLockID++; + locks.Add(nextLockID); + return nextLockID; + } + + void Unlock (int id) { + if (!locks.Remove(id)) { + throw new System.ArgumentException("This lock has already been released"); + } + + // Check if there are no remaining active locks + if (locks.Count == 0) { + if (OnQueueUnblocked != null) OnQueueUnblocked(); + + queue.isBlocked = false; + } + } + + /// <summary> + /// Prevents pathfinding threads from starting to calculate any new paths. + /// + /// Returns: A lock object. You need to call Unlock on that object to allow pathfinding to resume. + /// + /// Note: In most cases this should not be called from user code. + /// </summary> + /// <param name="block">If true, this call will block until all pathfinding threads are paused. + /// otherwise the threads will be paused as soon as they are done with what they are currently doing.</param> + public GraphUpdateLock PausePathfinding (bool block) { + return new GraphUpdateLock(this, block); + } + + /// <summary> + /// Does pathfinding calculations when not using multithreading. + /// + /// This method should be called once per frame if <see cref="IsUsingMultithreading"/> is true. + /// </summary> + public void TickNonMultithreaded () { + // Process paths + if (threadCoroutine == null) throw new System.InvalidOperationException("Cannot tick non-multithreaded pathfinding when no coroutine has been started"); + + try { + if (!threadCoroutine.MoveNext()) { + threadCoroutine = null; + coroutineReceiver.Close(); + } + } catch (System.Exception e) { + Debug.LogException(e); + Debug.LogError("Unhandled exception during pathfinding. Terminating."); + queue.Close(); + + // This will kill pathfinding + threadCoroutine = null; + coroutineReceiver.Close(); + } + } + + /// <summary> + /// Calls 'Join' on each of the threads to block until they have completed. + /// + /// This will also clean up any unmanaged memory used by the threads. + /// </summary> + public void StopThreads () { + // Don't accept any more path calls to this AstarPath instance. + // This will cause all pathfinding threads to exit (if any exist) + queue.Close(); + + if (threads != null) { + for (int i = 0; i < threads.Length; i++) { + if (!threads[i].Join(200)) { + Debug.LogError("Could not terminate pathfinding thread["+i+"] in 200ms, trying Thread.Abort"); + threads[i].Abort(); + } + } + threads = null; + } + if (threadCoroutine != null) { + Assert.IsTrue(queue.numReceivers > 0); + while (queue.numReceivers > 0) TickNonMultithreaded(); + Assert.IsNull(threadCoroutine); + } + + Assert.AreEqual(queue.numReceivers, 0, "Not all receivers were blocked and terminated when stopping threads"); + + // Dispose unmanaged data + for (int i = 0; i < pathHandlers.Length; i++) { + pathHandlers[i].Dispose(); + } + pathHandlers = new PathHandler[0]; + } + + /// <summary> + /// Cleans up all native memory managed by this instance. + /// + /// You may use this instance again by calling SetThreadCount. + /// </summary> + public void Dispose () { + StopThreads(); + } + + /// <summary> + /// Main pathfinding method (multithreaded). + /// This method will calculate the paths in the pathfinding queue when multithreading is enabled. + /// + /// See: CalculatePaths + /// See: <see cref="AstarPath.StartPath"/> + /// </summary> + void CalculatePathsThreaded (PathHandler pathHandler, BlockableChannel<Path>.Receiver receiver) { + UnityEngine.Profiling.Profiler.BeginThreadProfiling("Pathfinding", "Pathfinding thread #" + (pathHandler.threadID+1)); + + try { + // Max number of ticks we are allowed to continue working in one run. + // One tick is 1/10000 of a millisecond. + // We need to check once in a while if the thread should be stopped. + long maxTicks = (long)(10*10000); + long targetTick = System.DateTime.UtcNow.Ticks + maxTicks; + while (true) { + // The path we are currently calculating + if (receiver.Receive(out var path) == BlockableChannel<Path>.PopState.Closed) { + if (astar.logPathResults == PathLog.Heavy) + Debug.LogWarning("Shutting down pathfinding thread #" + pathHandler.threadID); + receiver.Close(); + return; + } + MarkerCalculatePath.Begin(); + // Access the internal implementation methods + IPathInternals ipath = (IPathInternals)path; + + + MarkerPreparePath.Begin(); + ipath.PrepareBase(pathHandler); + + // Now processing the path + // Will advance to Processing + ipath.AdvanceState(PathState.Processing); + + // Call some callbacks + if (OnPathPreSearch != null) { + OnPathPreSearch(path); + } + + // Tick for when the path started, used for calculating how long time the calculation took + long startTicks = System.DateTime.UtcNow.Ticks; + + // Prepare the path + ipath.Prepare(); + MarkerPreparePath.End(); + + + if (path.CompleteState == PathCompleteState.NotCalculated) { + // For visualization purposes, we set the last computed path to p, so we can view debug info on it in the editor (scene view). + astar.debugPathData = ipath.PathHandler; + astar.debugPathID = path.pathID; + + // Loop while the path has not been fully calculated + while (path.CompleteState == PathCompleteState.NotCalculated) { + // Do some work on the path calculation. + // The function will return when it has taken too much time + // or when it has finished calculation + ipath.CalculateStep(targetTick); + + targetTick = System.DateTime.UtcNow.Ticks + maxTicks; + + // Cancel function (and thus the thread) if no more paths should be accepted. + // This is done when the A* object is about to be destroyed + // The path is returned and then this function will be terminated + if (queue.isClosed) { + path.FailWithError("AstarPath object destroyed"); + } + } + + path.duration = (System.DateTime.UtcNow.Ticks - startTicks)*0.0001F; + +#if ProfileAstar + System.Threading.Interlocked.Increment(ref AstarPath.PathsCompleted); + System.Threading.Interlocked.Add(ref AstarPath.TotalSearchTime, System.DateTime.UtcNow.Ticks - startTicks); +#endif + } + + // Cleans up node tagging and other things + ipath.Cleanup(); + pathHandler.heap.Clear(pathHandler.pathNodes); + + + if (path.immediateCallback != null) path.immediateCallback(path); + + if (OnPathPostSearch != null) { + OnPathPostSearch(path); + } + + // Push the path onto the return stack + // It will be detected by the main Unity thread and returned as fast as possible (the next late update hopefully) + returnQueue.Enqueue(path); + + // Will advance to ReturnQueue + ipath.AdvanceState(PathState.ReturnQueue); + + MarkerCalculatePath.End(); + } + } catch (System.Exception e) { +#if !NETFX_CORE + if (e is ThreadAbortException) { + if (astar.logPathResults == PathLog.Heavy) + Debug.LogWarning("Shutting down pathfinding thread #" + pathHandler.threadID); + receiver.Close(); + return; + } +#endif + + Debug.LogException(e); + Debug.LogError("Unhandled exception during pathfinding. Terminating."); + // Unhandled exception, kill pathfinding + queue.Close(); + } finally { + UnityEngine.Profiling.Profiler.EndThreadProfiling(); + } + + Debug.LogError("Error : This part should never be reached."); + receiver.Close(); + } + + /// <summary> + /// Main pathfinding method. + /// This method will calculate the paths in the pathfinding queue. + /// + /// See: CalculatePathsThreaded + /// See: StartPath + /// </summary> + IEnumerator CalculatePaths (PathHandler pathHandler) { + // Max number of ticks before yielding/sleeping + long maxTicks = (long)(astar.maxFrameTime*10000); + long targetTick = System.DateTime.UtcNow.Ticks + maxTicks; + + while (true) { + // The path we are currently calculating + Path p = null; + + // Try to get the next path to be calculated + bool blockedBefore = false; + while (p == null) { + switch (coroutineReceiver.ReceiveNoBlock(blockedBefore, out p)) { + case BlockableChannel<Path>.PopState.Ok: + break; + case BlockableChannel<Path>.PopState.Wait: + blockedBefore = true; + yield return null; + break; + case BlockableChannel<Path>.PopState.Closed: + yield break; + } + } + + IPathInternals ip = (IPathInternals)p; + + // Max number of ticks we are allowed to use for pathfinding in one frame + // One tick is 1/10000 of a millisecond + maxTicks = (long)(astar.maxFrameTime*10000); + + ip.PrepareBase(pathHandler); + + // Now processing the path + // Will advance to Processing + ip.AdvanceState(PathState.Processing); + + // Call some callbacks + // It needs to be stored in a local variable to avoid race conditions + var tmpOnPathPreSearch = OnPathPreSearch; + if (tmpOnPathPreSearch != null) tmpOnPathPreSearch(p); + + // Tick for when the path started, used for calculating how long time the calculation took + long startTicks = System.DateTime.UtcNow.Ticks; + long totalTicks = 0; + + ip.Prepare(); + + // Check if the Prepare call caused the path to complete + // If this happens the path usually failed + if (p.CompleteState == PathCompleteState.NotCalculated) { + // For debug uses, we set the last computed path to p, so we can view debug info on it in the editor (scene view). + astar.debugPathData = ip.PathHandler; + astar.debugPathID = p.pathID; + + // The error can turn up in the Init function + while (p.CompleteState == PathCompleteState.NotCalculated) { + // Run some pathfinding calculations. + // The function will return when it has taken too much time + // or when it has finished calculating the path. + ip.CalculateStep(targetTick); + + + // If the path has finished calculating, we can break here directly instead of sleeping + // Improves latency + if (p.CompleteState != PathCompleteState.NotCalculated) break; + + totalTicks += System.DateTime.UtcNow.Ticks-startTicks; + // Yield/sleep so other threads can work + + yield return null; + + startTicks = System.DateTime.UtcNow.Ticks; + + // Cancel function (and thus the thread) if no more paths should be accepted. + // This is done when the A* object is about to be destroyed + // The path is returned and then this function will be terminated (see similar IF statement higher up in the function) + if (queue.isClosed) { + p.FailWithError("AstarPath object destroyed"); + } + + targetTick = System.DateTime.UtcNow.Ticks + maxTicks; + } + + totalTicks += System.DateTime.UtcNow.Ticks-startTicks; + p.duration = totalTicks*0.0001F; + +#if ProfileAstar + System.Threading.Interlocked.Increment(ref AstarPath.PathsCompleted); +#endif + } + + // Cleans up node tagging and other things + ip.Cleanup(); + pathHandler.heap.Clear(pathHandler.pathNodes); + + + // Call the immediate callback + // It needs to be stored in a local variable to avoid race conditions + var tmpImmediateCallback = p.immediateCallback; + if (tmpImmediateCallback != null) tmpImmediateCallback(p); + + + // It needs to be stored in a local variable to avoid race conditions + var tmpOnPathPostSearch = OnPathPostSearch; + if (tmpOnPathPostSearch != null) tmpOnPathPostSearch(p); + + + // Push the path onto the return stack + // It will be detected by the main Unity thread and returned as fast as possible (the next late update) + returnQueue.Enqueue(p); + + ip.AdvanceState(PathState.ReturnQueue); + + + // Wait a bit if we have calculated a lot of paths + if (System.DateTime.UtcNow.Ticks > targetTick) { + yield return null; + targetTick = System.DateTime.UtcNow.Ticks + maxTicks; + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathProcessor.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathProcessor.cs.meta new file mode 100644 index 0000000..764f634 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathProcessor.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: f1b519f8b6e2c450aaae5cb2df6c73be +timeCreated: 1443114816 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathReturnQueue.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathReturnQueue.cs new file mode 100644 index 0000000..196c39b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathReturnQueue.cs @@ -0,0 +1,85 @@ +using UnityEngine; +using System.Collections.Generic; +using UnityEngine.Profiling; + +namespace Pathfinding { + class PathReturnQueue { + /// <summary> + /// Holds all paths which are waiting to be flagged as completed. + /// See: <see cref="ReturnPaths"/> + /// </summary> + readonly Queue<Path> pathReturnQueue = new Queue<Path>(); + + /// <summary> + /// Paths are claimed silently by some object to prevent them from being recycled while still in use. + /// This will be set to the AstarPath object. + /// </summary> + readonly System.Object pathsClaimedSilentlyBy; + + readonly System.Action OnReturnedPaths; + + public PathReturnQueue (System.Object pathsClaimedSilentlyBy, System.Action OnReturnedPaths) { + this.pathsClaimedSilentlyBy = pathsClaimedSilentlyBy; + this.OnReturnedPaths = OnReturnedPaths; + } + + public void Enqueue (Path path) { + lock (pathReturnQueue) { + pathReturnQueue.Enqueue(path); + } + } + + /// <summary> + /// Returns all paths in the return stack. + /// Paths which have been processed are put in the return stack. + /// This function will pop all items from the stack and return them to e.g the Seeker requesting them. + /// </summary> + /// <param name="timeSlice">Do not return all paths at once if it takes a long time, instead return some and wait until the next call.</param> + public void ReturnPaths (bool timeSlice) { + Profiler.BeginSample("Calling Path Callbacks"); + + // Hard coded limit on 1.0 ms + long targetTick = timeSlice ? System.DateTime.UtcNow.Ticks + 1 * 10000 : 0; + + int counter = 0; + int totalReturned = 0; + // Loop through the linked list and return all paths + while (true) { + // Move to the next path + Path path; + lock (pathReturnQueue) { + if (pathReturnQueue.Count == 0) break; + path = pathReturnQueue.Dequeue(); + } + + // Will increment path state to Returned + ((IPathInternals)path).AdvanceState(PathState.Returning); + + try { + // Return the path + ((IPathInternals)path).ReturnPath(); + } catch (System.Exception e) { + Debug.LogException(e); + } + + // Will increment path state to Returned + ((IPathInternals)path).AdvanceState(PathState.Returned); + + path.Release(pathsClaimedSilentlyBy, true); + + counter++; + totalReturned++; + // At least 5 paths will be returned, even if timeSlice is enabled + if (counter > 5 && timeSlice) { + counter = 0; + if (System.DateTime.UtcNow.Ticks >= targetTick) { + break; + } + } + } + + if (totalReturned > 0) OnReturnedPaths(); + Profiler.EndSample(); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathReturnQueue.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathReturnQueue.cs.meta new file mode 100644 index 0000000..4181918 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Pathfinding/PathReturnQueue.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 70ca370ae38794366be2fa32880f362e +timeCreated: 1443114816 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: |