From 8722a9920c1f6119bf6e769cba270e63097f8e25 Mon Sep 17 00:00:00 2001 From: chai <215380520@qq.com> Date: Thu, 23 May 2024 10:08:29 +0800 Subject: + astar project --- .../Utilities/AnimationLinkTraverser.cs | 74 + .../Utilities/AnimationLinkTraverser.cs.meta | 12 + .../Utilities/AstarChecksum.cs | 21 + .../Utilities/AstarChecksum.cs.meta | 7 + .../Utilities/BatchedEvents.cs | 243 +++ .../Utilities/BatchedEvents.cs.meta | 11 + .../Utilities/DotNetReplacements.cs | 150 ++ .../Utilities/DotNetReplacements.cs.meta | 7 + .../Utilities/DynamicGridObstacle.cs | 238 +++ .../Utilities/DynamicGridObstacle.cs.meta | 11 + .../com.arongranberg.astar/Utilities/Funnel.cs | 883 +++++++++ .../Utilities/Funnel.cs.meta | 12 + .../Utilities/GraphGizmoHelper.cs | 247 +++ .../Utilities/GraphGizmoHelper.cs.meta | 12 + .../Utilities/GraphUpdateUtilities.cs | 104 + .../Utilities/GraphUpdateUtilities.cs.meta | 7 + .../Utilities/GridStringPulling.cs | 577 ++++++ .../Utilities/GridStringPulling.cs.meta | 11 + .../Utilities/IJobParallelForBatched.cs | 65 + .../Utilities/IJobParallelForBatched.cs.meta | 11 + .../Utilities/JobDependencyTracker.cs | 799 ++++++++ .../Utilities/JobDependencyTracker.cs.meta | 11 + .../Utilities/MeshUtility.cs | 120 ++ .../Utilities/MeshUtility.cs.meta | 11 + .../com.arongranberg.astar/Utilities/PathTracer.cs | 2071 ++++++++++++++++++++ .../Utilities/PathTracer.cs.meta | 11 + .../Utilities/PathUtilities.cs | 747 +++++++ .../Utilities/PathUtilities.cs.meta | 7 + .../Utilities/ProceduralGraphMover.cs | 257 +++ .../Utilities/ProceduralGraphMover.cs.meta | 11 + .../com.arongranberg.astar/Utilities/Promise.cs | 48 + .../Utilities/Promise.cs.meta | 11 + .../com.arongranberg.astar/Utilities/RWLock.cs | 215 ++ .../Utilities/RWLock.cs.meta | 11 + .../com.arongranberg.astar/Utilities/SpinLock.cs | 28 + .../Utilities/SpinLock.cs.meta | 11 + .../Utilities/UnityReferenceHelper.cs | 39 + .../Utilities/UnityReferenceHelper.cs.meta | 7 + 38 files changed, 7118 insertions(+) create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AnimationLinkTraverser.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AnimationLinkTraverser.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AstarChecksum.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AstarChecksum.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/BatchedEvents.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/BatchedEvents.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DotNetReplacements.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DotNetReplacements.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DynamicGridObstacle.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DynamicGridObstacle.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Funnel.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Funnel.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphUpdateUtilities.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphUpdateUtilities.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/IJobParallelForBatched.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/IJobParallelForBatched.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/JobDependencyTracker.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/JobDependencyTracker.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/MeshUtility.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/MeshUtility.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathTracer.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathTracer.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/ProceduralGraphMover.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/ProceduralGraphMover.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Promise.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Promise.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/RWLock.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/RWLock.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/SpinLock.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/SpinLock.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/UnityReferenceHelper.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/UnityReferenceHelper.cs.meta (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities') diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AnimationLinkTraverser.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AnimationLinkTraverser.cs new file mode 100644 index 0000000..455bc3e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AnimationLinkTraverser.cs @@ -0,0 +1,74 @@ +using System.Collections; +using UnityEngine; + +namespace Pathfinding.Examples { + using Pathfinding; + + /// + /// Example of how to handle off-mesh link traversal. + /// This is used in the "Example4_Recast_Navmesh2" example scene. + /// + /// See: + /// See: + /// See: + /// + [HelpURL("https://arongranberg.com/astar/documentation/stable/animationlinktraverser.html")] + public class AnimationLinkTraverser : VersionedMonoBehaviour { + public Animation anim; + + RichAI ai; + + void OnEnable () { + ai = GetComponent(); + if (ai != null) ai.onTraverseOffMeshLink += TraverseOffMeshLink; + } + + void OnDisable () { + if (ai != null) ai.onTraverseOffMeshLink -= TraverseOffMeshLink; + } + + protected virtual IEnumerator TraverseOffMeshLink (RichSpecial rs) { + var link = rs.nodeLink.gameObject.GetComponent(); + + if (link == null) { + Debug.LogError("Unhandled RichSpecial"); + yield break; + } + + // Rotate character to face the correct direction + while (true) { + var origRotation = ai.rotation; + var finalRotation = ai.SimulateRotationTowards(rs.first.rotation * Vector3.forward, ai.rotationSpeed * Time.deltaTime); + // Rotate until the rotation does not change anymore + if (origRotation == finalRotation) break; + ai.FinalizeMovement(ai.position, finalRotation); + yield return null; + } + + // Reposition + transform.parent.position = transform.position; + + transform.parent.rotation = transform.rotation; + transform.localPosition = Vector3.zero; + transform.localRotation = Quaternion.identity; + + // Set up animation speeds + if (rs.reverse && link.reverseAnim) { + anim[link.clip].speed = -link.animSpeed; + anim[link.clip].normalizedTime = 1; + anim.Play(link.clip); + anim.Sample(); + } else { + anim[link.clip].speed = link.animSpeed; + anim.Rewind(link.clip); + anim.Play(link.clip); + } + + // Fix required for animations in reverse direction + transform.parent.position -= transform.position-transform.parent.position; + + // Wait for the animation to finish + yield return new WaitForSeconds(Mathf.Abs(anim[link.clip].length/link.animSpeed)); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AnimationLinkTraverser.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AnimationLinkTraverser.cs.meta new file mode 100644 index 0000000..8f98e0a --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AnimationLinkTraverser.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 81825518f1bed49bda1223459d4bdf99 +timeCreated: 1498659013 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AstarChecksum.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AstarChecksum.cs new file mode 100644 index 0000000..b492542 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AstarChecksum.cs @@ -0,0 +1,21 @@ +using System; +namespace Pathfinding.Util { + /// Calculates checksums of byte arrays + public class Checksum { + /// + /// Calculate checksum for the byte array starting from a previous values. + /// Useful if data is split up between several byte arrays + /// + public static uint GetChecksum (byte[] arr, uint hash) { + // Sort of implements the Fowler–Noll–Vo hash function + const int prime = 16777619; + + hash ^= 2166136261U; + + for (int i = 0; i < arr.Length; i++) + hash = (hash ^ arr[i]) * prime; + + return hash; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AstarChecksum.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AstarChecksum.cs.meta new file mode 100644 index 0000000..0093682 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/AstarChecksum.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 8959628b7b47f4ccca91bcfa87d9f77e +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/BatchedEvents.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/BatchedEvents.cs new file mode 100644 index 0000000..f8a0a7e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/BatchedEvents.cs @@ -0,0 +1,243 @@ +using Unity.Mathematics; +using UnityEngine; +using UnityEngine.Jobs; +using UnityEngine.Profiling; + +namespace Pathfinding.Util { + /// Helper for batching updates to many objects efficiently + [HelpURL("https://arongranberg.com/astar/documentation/stable/batchedevents.html")] + public class BatchedEvents : VersionedMonoBehaviour { + const int ArchetypeOffset = 22; + const int ArchetypeMask = 0xFF << ArchetypeOffset; + + static Archetype[] data = new Archetype[0]; + static BatchedEvents instance; + static int isIteratingOverTypeIndex = -1; + static bool isIterating = false; + + [System.Flags] + public enum Event { + Update = 1 << 0, + LateUpdate = 1 << 1, + FixedUpdate = 1 << 2, + Custom = 1 << 3, + None = 0, + }; + + + struct Archetype { + public object[] objects; + public int objectCount; + public System.Type type; + public TransformAccessArray transforms; + public int variant; + public int archetypeIndex; + public Event events; + public System.Action action; + public CustomSampler sampler; + + public void Add (Component obj) { + objectCount++; + UnityEngine.Assertions.Assert.IsTrue(objectCount < (1 << ArchetypeOffset)); + if (objects == null) objects = (object[])System.Array.CreateInstance(type, math.ceilpow2(objectCount)); + if (objectCount > objects.Length) { + var newObjects = System.Array.CreateInstance(type, math.ceilpow2(objectCount)); + objects.CopyTo(newObjects, 0); + objects = (object[])newObjects; + } + objects[objectCount-1] = obj; + if (!transforms.isCreated) transforms = new TransformAccessArray(16, -1); + transforms.Add(obj.transform); + ((IEntityIndex)obj).EntityIndex = (archetypeIndex << ArchetypeOffset) | (objectCount-1); + } + + public void Remove (int index) { + objectCount--; + ((IEntityIndex)objects[objectCount]).EntityIndex = (archetypeIndex << ArchetypeOffset) | index; + ((IEntityIndex)objects[index]).EntityIndex = 0; + objects[index] = objects[objectCount]; + objects[objectCount] = null; + transforms.RemoveAtSwapBack(index); + + if (objectCount == 0) transforms.Dispose(); + } + } + +#if UNITY_EDITOR + void DelayedDestroy () { + UnityEditor.EditorApplication.update -= DelayedDestroy; + GameObject.DestroyImmediate(gameObject); + } +#endif + + void OnEnable () { + if (instance == null) instance = this; + if (instance != this) { + // We cannot destroy the object while it is being enabled, so we need to delay it a bit +#if UNITY_EDITOR + // This is only important in the editor to avoid a build-up of old managers. + // In an actual game at most 1 (though in practice zero) old managers will be laying around. + // It would be nice to use a coroutine for this instead, but unfortunately they do not work for objects marked with HideAndDontSave. + UnityEditor.EditorApplication.update += DelayedDestroy; +#endif + } + } + + static void CreateInstance () { + // If scripts are recompiled the the static variable will be lost. + // Some users recompile scripts in play mode and then reload the scene (https://forum.arongranberg.com/t/rts-game-pathfinding/6623/48?u=aron_granberg) + // which makes handling this a requirement. + + // Here one might try to look for existing instances of the class that haven't yet been enabled. + // However, this turns out to be tricky. + // Resources.FindObjectsOfTypeAll() is the only call that includes HideInInspector GameObjects. + // But it is hard to distinguish between objects that are internal ones which will never be enabled and objects that will be enabled. + // Checking .gameObject.scene.isLoaded doesn't work reliably (object may be enabled and working even if isLoaded is false) + // Checking .gameObject.scene.isValid doesn't work reliably (object may be enabled and working even if isValid is false) + + // So instead we just always create a new instance. This is not a particularly heavy operation and it only happens once per game, so why not. + // The OnEnable call will clean up duplicate managers if there are any. + + var go = new GameObject("Batch Helper") { + hideFlags = HideFlags.DontSave | HideFlags.NotEditable | HideFlags.HideInInspector | HideFlags.HideInHierarchy + }; + + instance = go.AddComponent(); + DontDestroyOnLoad(go); + } + + public static T Find(K key, System.Func predicate) where T : class, IEntityIndex { + var t = typeof(T); + for (int i = 0; i < data.Length; i++) { + if (data[i].type == t) { + var objs = data[i].objects as T[]; + for (int j = 0; j < data[i].objectCount; j++) { + if (predicate(objs[j], key)) return objs[j]; + } + } + } + return null; + } + + public static void Remove(T obj) where T : IEntityIndex { + int index = obj.EntityIndex; + + if (index == 0) return; + + var archetypeIndex = ((index & ArchetypeMask) >> ArchetypeOffset) - 1; + index &= ~ArchetypeMask; + UnityEngine.Assertions.Assert.IsTrue(data[archetypeIndex].type == obj.GetType()); + + if (isIterating && isIteratingOverTypeIndex == archetypeIndex) throw new System.Exception("Cannot add or remove entities during an event (Update/LateUpdate/...) that this helper initiated"); + data[archetypeIndex].Remove(index); + } + + public static int GetComponents(Event eventTypes, out TransformAccessArray transforms, out T[] components) where T : Component, IEntityIndex { + if (instance == null) CreateInstance(); + + // Add in a hash of the event types + var archetypeVariant = (int)eventTypes * 12582917; + if (isIterating && isIteratingOverTypeIndex == archetypeVariant) throw new System.Exception("Cannot add or remove entities during an event (Update/LateUpdate/...) that this helper initiated"); + + var type = typeof(T); + for (int i = 0; i < data.Length; i++) { + if (data[i].type == type && data[i].variant == archetypeVariant) { + transforms = data[i].transforms; + components = data[i].objects as T[]; + return data[i].objectCount; + } + } + + transforms = default; + components = null; + return 0; + } + + public static bool Has(T obj) where T : IEntityIndex => obj.EntityIndex != 0; + + public static void Add(T obj, Event eventTypes, System.Action action, int archetypeVariant = 0) where T : Component, IEntityIndex { + Add(obj, eventTypes, null, action, archetypeVariant); + } + + public static void Add(T obj, Event eventTypes, System.Action action, int archetypeVariant = 0) where T : Component, IEntityIndex { + Add(obj, eventTypes, action, null, archetypeVariant); + } + + static void Add(T obj, Event eventTypes, System.Action action1, System.Action action2, int archetypeVariant = 0) where T : Component, IEntityIndex { + if (obj.EntityIndex != 0) { + throw new System.ArgumentException("This object is already registered. Call Remove before adding the object again."); + } + + if (instance == null) CreateInstance(); + + // Add in a hash of the event types + archetypeVariant = (int)eventTypes * 12582917; + if (isIterating && isIteratingOverTypeIndex == archetypeVariant) throw new System.Exception("Cannot add or remove entities during an event (Update/LateUpdate/...) that this helper initiated"); + + + var type = obj.GetType(); + for (int i = 0; i < data.Length; i++) { + if (data[i].type == type && data[i].variant == archetypeVariant) { + data[i].Add(obj); + return; + } + } + + { + Memory.Realloc(ref data, data.Length + 1); + // A copy is made here so that these variables are captured by the lambdas below instead of the original action1/action2 parameters. + // If this is not done then the C# JIT will allocate a lambda capture object every time this function is executed + // instead of only when we need to create a new archetype. Doing that would create a lot more unnecessary garbage. + var ac1 = action1; + var ac2 = action2; + System.Action a1 = (objs, count, tr, ev) => ac1((T[])objs, count, tr, ev); + System.Action a2 = (objs, count, tr, ev) => ac2((T[])objs, count); + data[data.Length - 1] = new Archetype { + type = type, + events = eventTypes, + variant = archetypeVariant, + archetypeIndex = (data.Length - 1) + 1, // Note: offset by +1 to ensure that entity index = 0 is an invalid index + action = ac1 != null ? a1 : a2, + sampler = CustomSampler.Create(type.Name), + }; + data[data.Length - 1].Add(obj); + } + } + + void Process (Event eventType, System.Type typeFilter) { + try { + isIterating = true; + for (int i = 0; i < data.Length; i++) { + ref var archetype = ref data[i]; + if (archetype.objectCount > 0 && (archetype.events & eventType) != 0 && (typeFilter == null || typeFilter == archetype.type)) { + isIteratingOverTypeIndex = archetype.variant; + try { + archetype.sampler.Begin(); + archetype.action(archetype.objects, archetype.objectCount, archetype.transforms, eventType); + } finally { + archetype.sampler.End(); + } + } + } + } finally { + isIterating = false; + } + } + + public static void ProcessEvent(Event eventType) { + instance?.Process(eventType, typeof(T)); + } + + void Update () { + Process(Event.Update, null); + } + + void LateUpdate () { + Process(Event.LateUpdate, null); + } + + void FixedUpdate () { + Process(Event.FixedUpdate, null); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/BatchedEvents.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/BatchedEvents.cs.meta new file mode 100644 index 0000000..d1ae5f6 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/BatchedEvents.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 74164bd7b3a34430db5f19fbdf70c375 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DotNetReplacements.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DotNetReplacements.cs new file mode 100644 index 0000000..17f070e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DotNetReplacements.cs @@ -0,0 +1,150 @@ +namespace Pathfinding.Util { + /// + /// Simple implementation of a GUID. + /// Version: Since 3.6.4 this struct works properly on platforms with different endianness such as Wii U. + /// + public struct Guid { + const string hex = "0123456789ABCDEF"; + + public static readonly Guid zero = new Guid(new byte[16]); + public static readonly string zeroString = new Guid(new byte[16]).ToString(); + + readonly ulong _a, _b; + + public Guid (byte[] bytes) { + // Pack 128 bits into 2 longs + ulong a = ((ulong)bytes[0] << 8*0) | + ((ulong)bytes[1] << 8*1) | + ((ulong)bytes[2] << 8*2) | + ((ulong)bytes[3] << 8*3) | + ((ulong)bytes[4] << 8*4) | + ((ulong)bytes[5] << 8*5) | + ((ulong)bytes[6] << 8*6) | + ((ulong)bytes[7] << 8*7); + + ulong b = ((ulong)bytes[8] << 8*0) | + ((ulong)bytes[9] << 8*1) | + ((ulong)bytes[10] << 8*2) | + ((ulong)bytes[11] << 8*3) | + ((ulong)bytes[12] << 8*4) | + ((ulong)bytes[13] << 8*5) | + ((ulong)bytes[14] << 8*6) | + ((ulong)bytes[15] << 8*7); + + // Need to swap endianness on e.g Wii U + _a = System.BitConverter.IsLittleEndian ? a : SwapEndianness(a); + _b = System.BitConverter.IsLittleEndian ? b : SwapEndianness(b); + } + + public Guid (string str) { + _a = 0; + _b = 0; + + if (str.Length < 32) + throw new System.FormatException("Invalid Guid format"); + + int counter = 0; + int i = 0; + int offset = 15*4; + + for (; counter < 16; i++) { + if (i >= str.Length) + throw new System.FormatException("Invalid Guid format. String too short"); + + char c = str[i]; + if (c == '-') continue; + + //Neat trick, perhaps a bit slow, but one will probably not use Guid parsing that much + int value = hex.IndexOf(char.ToUpperInvariant(c)); + if (value == -1) + throw new System.FormatException("Invalid Guid format : "+c+" is not a hexadecimal character"); + + _a |= (ulong)value << offset; + //SetByte (counter,(byte)value); + offset -= 4; + counter++; + } + + offset = 15*4; + for (; counter < 32; i++) { + if (i >= str.Length) + throw new System.FormatException("Invalid Guid format. String too short"); + + char c = str[i]; + if (c == '-') continue; + + //Neat trick, perhaps a bit slow, but one will probably not use Guid parsing that much + int value = hex.IndexOf(char.ToUpperInvariant(c)); + if (value == -1) + throw new System.FormatException("Invalid Guid format : "+c+" is not a hexadecimal character"); + + _b |= (ulong)value << offset; + //SetByte (counter,(byte)value); + offset -= 4; + counter++; + } + } + + public static Guid Parse (string input) { + return new Guid(input); + } + + /// Swaps between little and big endian + static ulong SwapEndianness (ulong value) { + var b1 = (value >> 0) & 0xff; + var b2 = (value >> 8) & 0xff; + var b3 = (value >> 16) & 0xff; + var b4 = (value >> 24) & 0xff; + var b5 = (value >> 32) & 0xff; + var b6 = (value >> 40) & 0xff; + var b7 = (value >> 48) & 0xff; + var b8 = (value >> 56) & 0xff; + + return b1 << 56 | b2 << 48 | b3 << 40 | b4 << 32 | b5 << 24 | b6 << 16 | b7 << 8 | b8 << 0; + } + + private static System.Random random = new System.Random(); + + public static Guid NewGuid () { + var bytes = new byte[16]; + + random.NextBytes(bytes); + return new Guid(bytes); + } + + public static bool operator == (Guid lhs, Guid rhs) { + return lhs._a == rhs._a && lhs._b == rhs._b; + } + + public static bool operator != (Guid lhs, Guid rhs) { + return lhs._a != rhs._a || lhs._b != rhs._b; + } + + public override bool Equals (System.Object _rhs) { + if (!(_rhs is Guid)) return false; + + var rhs = (Guid)_rhs; + + return _a == rhs._a && _b == rhs._b; + } + + public override int GetHashCode () { + ulong ab = _a ^ _b; + + return (int)(ab >> 32) ^ (int)ab; + } + + private static System.Text.StringBuilder text; + + public override string ToString () { + if (text == null) { + text = new System.Text.StringBuilder(); + } + lock (text) { + text.Length = 0; + text.Append(_a.ToString("x16")).Append('-').Append(_b.ToString("x16")); + return text.ToString(); + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DotNetReplacements.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DotNetReplacements.cs.meta new file mode 100644 index 0000000..da6aacd --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DotNetReplacements.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: c306b1bfc824f40909c5a91e06e4090f +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DynamicGridObstacle.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DynamicGridObstacle.cs new file mode 100644 index 0000000..e6be96d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DynamicGridObstacle.cs @@ -0,0 +1,238 @@ +using UnityEngine; +using System.Collections.Generic; + +namespace Pathfinding { + /// + /// Attach this script to any obstacle with a collider to enable dynamic updates of the graphs around it. + /// When the object has moved or rotated at least world units + /// then it will call AstarPath.UpdateGraphs and update the graph around it. + /// + /// Make sure that any children colliders do not extend beyond the bounds of the collider attached to the + /// GameObject that the DynamicGridObstacle component is attached to since this script only updates the graph + /// around the bounds of the collider on the same GameObject. + /// + /// An update will be triggered whenever the bounding box of the attached collider has changed (moved/expanded/etc.) by at least world units or if + /// the GameObject has rotated enough so that the outmost point of the object has moved at least world units. + /// + /// This script works with both 2D colliders and normal 3D colliders. + /// + /// Note: This script works with a GridGraph, PointGraph, LayerGridGraph or RecastGraph. + /// However, for recast graphs, you can often use the instead, for simple obstacles. The can be faster, but it's not quite as flexible. + /// + /// See: AstarPath.UpdateGraphs + /// See: graph-updates (view in online documentation for working links) + /// See: navmeshcutting (view in online documentation for working links) + /// + [AddComponentMenu("Pathfinding/Dynamic Grid Obstacle")] + [HelpURL("https://arongranberg.com/astar/documentation/stable/dynamicgridobstacle.html")] + public class DynamicGridObstacle : GraphModifier { + /// Collider to get bounds information from + Collider coll; + + /// 2D Collider to get bounds information from + Collider2D coll2D; + + /// Cached transform component + Transform tr; + + /// The minimum change in world units along one of the axis of the bounding box of the collider to trigger a graph update + public float updateError = 1; + + /// + /// Time in seconds between bounding box checks. + /// If AstarPath.batchGraphUpdates is enabled, it is not beneficial to have a checkTime much lower + /// than AstarPath.graphUpdateBatchingInterval because that will just add extra unnecessary graph updates. + /// + /// In real time seconds (based on Time.realtimeSinceStartup). + /// + public float checkTime = 0.2F; + + /// Bounds of the collider the last time the graphs were updated + Bounds prevBounds; + + /// Rotation of the collider the last time the graphs were updated + Quaternion prevRotation; + + /// True if the collider was enabled last time the graphs were updated + bool prevEnabled; + + float lastCheckTime = -9999; + Queue pendingGraphUpdates = new Queue(); + + Bounds bounds { + get { + if (coll != null) { + return coll.bounds; + } else if (coll2D != null) { + var b = coll2D.bounds; + // Make sure the bounding box stretches close to infinitely along the Z axis (which is the axis perpendicular to the 2D plane). + // We don't want any change along the Z axis to make a difference. + b.extents += new Vector3(0, 0, 10000); + return b; + } else { + return default; + } + } + } + + bool colliderEnabled { + get { + return coll != null ? coll.enabled : coll2D.enabled; + } + } + + protected override void Awake () { + base.Awake(); + + coll = GetComponent(); + coll2D = GetComponent(); + tr = transform; + if (coll == null && coll2D == null && Application.isPlaying) { + throw new System.Exception("A collider or 2D collider must be attached to the GameObject(" + gameObject.name + ") for the DynamicGridObstacle to work"); + } + + prevBounds = bounds; + prevRotation = tr.rotation; + // Make sure we update the graph as soon as we find that the collider is enabled + prevEnabled = false; + } + + public override void OnPostScan () { + // Make sure we find the collider + // AstarPath.Awake may run before Awake on this component + if (coll == null) Awake(); + + // In case the object was in the scene from the start and the graphs + // were scanned then we ignore the first update since it is unnecessary. + if (coll != null) prevEnabled = colliderEnabled; + } + + void Update () { + if (!Application.isPlaying) return; + + if (coll == null && coll2D == null) { + Debug.LogError("No collider attached to this GameObject. The DynamicGridObstacle component requires a collider.", this); + enabled = false; + return; + } + + // Check if the previous graph updates have been completed yet. + // We don't want to update the graph again until the last graph updates are done. + // This is particularly important for recast graphs for which graph updates can take a long time. + while (pendingGraphUpdates.Count > 0 && pendingGraphUpdates.Peek().stage != GraphUpdateStage.Pending) { + pendingGraphUpdates.Dequeue(); + } + + if (AstarPath.active == null || AstarPath.active.isScanning || Time.realtimeSinceStartup - lastCheckTime < checkTime || !Application.isPlaying || pendingGraphUpdates.Count > 0) { + return; + } + + lastCheckTime = Time.realtimeSinceStartup; + if (colliderEnabled) { + // The current bounds of the collider + Bounds newBounds = bounds; + var newRotation = tr.rotation; + + Vector3 minDiff = prevBounds.min - newBounds.min; + Vector3 maxDiff = prevBounds.max - newBounds.max; + + var extents = newBounds.extents.magnitude; + // This is the distance that a point furthest out on the bounding box + // would have moved due to the changed rotation of the object + var errorFromRotation = extents*Quaternion.Angle(prevRotation, newRotation)*Mathf.Deg2Rad; + + // If the difference between the previous bounds and the new bounds is greater than some value, update the graphs + if (minDiff.sqrMagnitude > updateError*updateError || maxDiff.sqrMagnitude > updateError*updateError || + errorFromRotation > updateError || !prevEnabled) { + // Update the graphs as soon as possible + DoUpdateGraphs(); + } + } else { + // Collider has just been disabled + if (prevEnabled) { + DoUpdateGraphs(); + } + } + } + + /// + /// Revert graphs when disabled. + /// When the DynamicObstacle is disabled or destroyed, a last graph update should be done to revert nodes to their original state + /// + protected override void OnDisable () { + base.OnDisable(); + if (AstarPath.active != null && Application.isPlaying) { + var guo = new GraphUpdateObject(prevBounds); + pendingGraphUpdates.Enqueue(guo); + AstarPath.active.UpdateGraphs(guo); + prevEnabled = false; + } + + // Stop caring about pending graph updates if this object is disabled. + // This avoids a memory leak since `Update` will never be called again to remove pending updates + // that have been completed. + pendingGraphUpdates.Clear(); + } + + /// + /// Update the graphs around this object. + /// Note: The graphs will not be updated immediately since the pathfinding threads need to be paused first. + /// If you want to guarantee that the graphs have been updated then call + /// after the call to this method. + /// + public void DoUpdateGraphs () { + if (coll == null && coll2D == null) return; + + // Required to ensure we get the most up to date bounding box from the physics engine + UnityEngine.Physics.SyncTransforms(); + UnityEngine.Physics2D.SyncTransforms(); + + if (!colliderEnabled) { + // If the collider is not enabled, then col.bounds will empty + // so just update prevBounds + var guo = new GraphUpdateObject(prevBounds); + pendingGraphUpdates.Enqueue(guo); + AstarPath.active.UpdateGraphs(guo); + } else { + Bounds newBounds = bounds; + + Bounds merged = newBounds; + merged.Encapsulate(prevBounds); + + // Check what seems to be fastest, to update the union of prevBounds and newBounds in a single request + // or to update them separately, the smallest volume is usually the fastest + if (BoundsVolume(merged) < BoundsVolume(newBounds) + BoundsVolume(prevBounds)) { + // Send an update request to update the nodes inside the 'merged' volume + var guo = new GraphUpdateObject(merged); + pendingGraphUpdates.Enqueue(guo); + AstarPath.active.UpdateGraphs(guo); + } else { + // Send two update request to update the nodes inside the 'prevBounds' and 'newBounds' volumes + var guo1 = new GraphUpdateObject(prevBounds); + var guo2 = new GraphUpdateObject(newBounds); + pendingGraphUpdates.Enqueue(guo1); + pendingGraphUpdates.Enqueue(guo2); + AstarPath.active.UpdateGraphs(guo1); + AstarPath.active.UpdateGraphs(guo2); + } + +#if ASTARDEBUG + Debug.DrawLine(prevBounds.min, prevBounds.max, Color.yellow); + Debug.DrawLine(newBounds.min, newBounds.max, Color.red); +#endif + prevBounds = newBounds; + } + + prevEnabled = colliderEnabled; + prevRotation = tr.rotation; + + // Set this here as well since the DoUpdateGraphs method can be called from other scripts + lastCheckTime = Time.realtimeSinceStartup; + } + + /// Volume of a Bounds object. X*Y*Z + static float BoundsVolume (Bounds b) { + return System.Math.Abs(b.size.x * b.size.y * b.size.z); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DynamicGridObstacle.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DynamicGridObstacle.cs.meta new file mode 100644 index 0000000..f5412aa --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/DynamicGridObstacle.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 183748dd497aa473a98ff0cd8cb67fc5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: -220 + icon: {fileID: 2800000, guid: edb61d815bfcfa548b5e947d759e9fb6, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Funnel.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Funnel.cs new file mode 100644 index 0000000..412b111 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Funnel.cs @@ -0,0 +1,883 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using System.Linq; +using UnityEngine.Assertions; + +namespace Pathfinding { + using Pathfinding.Util; + using Unity.Burst; + using Unity.Collections; + using Unity.Collections.LowLevel.Unsafe; + using Unity.Jobs; + using Unity.Mathematics; + using UnityEngine.Profiling; + + /// + /// Implements the funnel algorithm as well as various related methods. + /// See: http://digestingduck.blogspot.se/2010/03/simple-stupid-funnel-algorithm.html + /// See: Usually you do not use this class directly. Instead use the component. + /// + /// + /// using UnityEngine; + /// using Pathfinding; + /// using Pathfinding.Drawing; + /// + /// public class FunnelExample : MonoBehaviour { + /// public Transform target = null; + /// + /// void Update () { + /// var path = ABPath.Construct(transform.position, target.position); + /// + /// AstarPath.StartPath(path); + /// path.BlockUntilCalculated(); + /// + /// // Apply some default adjustments to the path + /// // not necessary if you are using the Seeker component + /// new StartEndModifier().Apply(path); + /// + /// // Split the path into segments and links + /// var parts = Funnel.SplitIntoParts(path); + /// // Optionally simplify the path to make it straighter + /// var nodes = path.path; + /// Funnel.Simplify(parts, ref nodes); + /// + /// using (Draw.WithLineWidth(2)) { + /// // Go through all the parts and draw them in the scene view + /// for (int i = 0; i < parts.Count; i++) { + /// var part = parts[i]; + /// if (part.type == Funnel.PartType.OffMeshLink) { + /// // Draw off-mesh links as a single line + /// Draw.Line(part.startPoint, part.endPoint, Color.cyan); + /// } else { + /// // Calculate the shortest path through the funnel + /// var portals = Funnel.ConstructFunnelPortals(nodes, part); + /// var pathThroghPortals = Funnel.Calculate(portals, splitAtEveryPortal: false); + /// Draw.Polyline(pathThroghPortals, Color.black); + /// } + /// } + /// } + /// } + /// } + /// + /// + /// In the image you can see the output from the code example above. The cyan lines represent off-mesh links. + /// + /// [Open online documentation to see images] + /// + [BurstCompile] + public static class Funnel { + /// Funnel in which the path to the target will be + public struct FunnelPortals { + public List left; + public List right; + } + + /// The type of a + public enum PartType { + /// An off-mesh link between two nodes in the same or different graphs + OffMeshLink, + /// A sequence of adjacent nodes in the same graph + NodeSequence, + } + + /// + /// Part of a path. + /// This is either a sequence of adjacent triangles + /// or a link. + /// See: NodeLink2 + /// + public struct PathPart { + /// Index of the first node in this part + public int startIndex; + /// Index of the last node in this part + public int endIndex; + /// Exact start-point of this part or off-mesh link + public Vector3 startPoint; + /// Exact end-point of this part or off-mesh link + public Vector3 endPoint; + /// If this is an off-mesh link or a sequence of nodes in a single graph + public PartType type; + } + + /// Splits the path into a sequence of parts which are either off-mesh links or sequences of adjacent triangles + + public static List SplitIntoParts (Path path) { + var nodes = path.path; + + var result = ListPool.Claim(); + + if (nodes == null || nodes.Count == 0) { + return result; + } + + // Loop through the path and split it into + // parts joined by links + for (int i = 0; i < nodes.Count; i++) { + var node = nodes[i]; + if (node is TriangleMeshNode || node is GridNodeBase) { + var startIndex = i; + uint currentGraphIndex = node.GraphIndex; + + // Loop up until we find a node in another graph + // Ignore NodeLink3 nodes + while (i < nodes.Count && (nodes[i].GraphIndex == currentGraphIndex || nodes[i] is NodeLink3Node)) i++; + + i--; + var endIndex = i; + result.Add(new PathPart { + type = PartType.NodeSequence, + startIndex = startIndex, + endIndex = endIndex, + // If this is the first part in the path, use the exact start point + // otherwise use the position of the node right before the start of this + // part which is likely the end of the link to this part + startPoint = startIndex == 0 ? path.vectorPath[0] : (Vector3)nodes[startIndex-1].position, + endPoint = endIndex == nodes.Count-1 ? path.vectorPath[path.vectorPath.Count-1] : (Vector3)nodes[endIndex+1].position, + }); + } else if (node is LinkNode) { + var startIndex = i; + var currentGraphIndex = node.GraphIndex; + + while (i < nodes.Count && nodes[i].GraphIndex == currentGraphIndex) i++; + i--; + + if (i - startIndex == 0) { + // The link is a single node. + // Just ignore it. It can happen in very rare circumstances with some path types. + // For example, a RandomPath can stop at the first node of a node link, without including the other end of the link. + + if (startIndex > 0 && startIndex + 1 < nodes.Count && nodes[startIndex - 1] == nodes[startIndex + 1]) { + // We can also move to a node link node and then immediately move back to the previous node in rare circumstances. + // Since triangle nodes are represented as 3 nodes during pathfinding, this is a possibility. + // (TODO: How can this happen in practice? It has been empirically observed on a standard graph, but the edge costs must be kinda weird for it to happen?) + + // [A, LinkNode, A] => [A] + nodes.RemoveRange(startIndex, 2); + i--; + throw new System.Exception("Link node connected back to the previous node in the path. This should not happen."); + } else { + // [A, LinkNode] => [A] + // [LinkNode, A] => [A] + Assert.IsTrue(startIndex == 0 || startIndex == nodes.Count - 1); + nodes.RemoveAt(startIndex); + i--; + } + + continue; + } else if (i - startIndex != 1) { + throw new System.Exception("Off mesh link included more than two nodes: " + (i - startIndex + 1)); + } + + result.Add(new PathPart { + type = PartType.OffMeshLink, + startIndex = startIndex, + endIndex = i, + startPoint = (Vector3)nodes[startIndex].position, + endPoint = (Vector3)nodes[i].position, + }); + } else { + throw new System.Exception("Unsupported node type or null node"); + } + } + + // The path should always start and stop on regular nodes + if (result[0].type == PartType.OffMeshLink) { + result.RemoveAt(0); + } + if (result[result.Count - 1].type == PartType.OffMeshLink) { + result.RemoveAt(result.Count - 1); + } + + Assert.IsTrue(result.Count > 0); + Assert.AreEqual(result[0].startIndex, 0); + Assert.AreEqual(result[0].type, PartType.NodeSequence); + Assert.AreEqual(result[result.Count-1].type, PartType.NodeSequence); + Assert.AreEqual(result[result.Count-1].endIndex, nodes.Count - 1); + + return result; + } + + + public static void Simplify (List parts, ref List nodes) { + List resultNodes = ListPool.Claim(); + + for (int i = 0; i < parts.Count; i++) { + var part = parts[i]; + + // We are changing the nodes list, so indices may change + var newPart = part; + newPart.startIndex = resultNodes.Count; + + if (part.type == PartType.NodeSequence) { + if (nodes[part.startIndex].Graph is IRaycastableGraph graph) { + Simplify(part, graph, nodes, resultNodes, Path.ZeroTagPenalties, -1); + newPart.endIndex = resultNodes.Count - 1; + parts[i] = newPart; + continue; + } + } + + for (int j = part.startIndex; j <= part.endIndex; j++) { + resultNodes.Add(nodes[j]); + } + newPart.endIndex = resultNodes.Count - 1; + parts[i] = newPart; + } + + ListPool.Release(ref nodes); + nodes = resultNodes; + } + + /// + /// Simplifies a funnel path using linecasting. + /// Running time is roughly O(n^2 log n) in the worst case (where n = end-start) + /// Actually it depends on how the graph looks, so in theory the actual upper limit on the worst case running time is O(n*m log n) (where n = end-start and m = nodes in the graph) + /// but O(n^2 log n) is a much more realistic worst case limit. + /// + /// Requires graph to implement IRaycastableGraph + /// + public static void Simplify (PathPart part, IRaycastableGraph graph, List nodes, List result, int[] tagPenalties, int traversableTags) { + var start = part.startIndex; + var end = part.endIndex; + var startPoint = part.startPoint; + var endPoint = part.endPoint; + + if (graph == null) throw new System.ArgumentNullException(nameof(graph)); + + if (start > end) { + throw new System.ArgumentException("start > end"); + } + + // Do a straight line of sight check to see if the path can be simplified to a single line + { + if (!graph.Linecast(startPoint, endPoint, out GraphHitInfo hit) && hit.node == nodes[end]) { + graph.Linecast(startPoint, endPoint, out hit, result); + + long penaltySum = 0; + long penaltySum2 = 0; + for (int i = start; i <= end; i++) { + penaltySum += nodes[i].Penalty + tagPenalties[nodes[i].Tag]; + } + + bool walkable = true; + for (int i = 0; i < result.Count; i++) { + penaltySum2 += result[i].Penalty + tagPenalties[result[i].Tag]; + walkable &= ((traversableTags >> (int)result[i].Tag) & 1) == 1; + } + + // Allow 40% more penalty on average per node + if (!walkable || (penaltySum*1.4*result.Count) < (penaltySum2*(end-start+1))) { + // The straight line penalties are much higher than the original path. + // Revert the simplification + result.Clear(); + } else { + // The straight line simplification looks good. + // We are done here. + return; + } + } + } + + int ostart = start; + + int count = 0; + while (true) { + if (count++ > 1000) { + Debug.LogError("Was the path really long or have we got cought in an infinite loop?"); + break; + } + + if (start == end) { + result.Add(nodes[end]); + return; + } + + int resCount = result.Count; + + // Run a binary search to find the furthest node that we have a clear line of sight to + int mx = end+1; + int mn = start+1; + bool anySucceded = false; + while (mx > mn+1) { + int mid = (mx+mn)/2; + + Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position; + Vector3 ep = mid == end ? endPoint : (Vector3)nodes[mid].position; + + // Check if there is an obstacle between these points, or if there is no obstacle, but we didn't end up at the right node. + // The second case can happen for example in buildings with multiple floors. + if (graph.Linecast(sp, ep, out GraphHitInfo hit) || hit.node != nodes[mid]) { + mx = mid; + } else { + anySucceded = true; + mn = mid; + } + } + + if (!anySucceded) { + result.Add(nodes[start]); + + // It is guaranteed that mn = start+1 + start = mn; + } else { + // Replace a part of the path with the straight path to the furthest node we had line of sight to. + // Need to redo the linecast to get the trace (i.e. list of nodes along the line of sight). + Vector3 sp = start == ostart ? startPoint : (Vector3)nodes[start].position; + Vector3 ep = mn == end ? endPoint : (Vector3)nodes[mn].position; + graph.Linecast(sp, ep, out _, result); + + long penaltySum = 0; + long penaltySum2 = 0; + for (int i = start; i <= mn; i++) { + penaltySum += nodes[i].Penalty + tagPenalties[nodes[i].Tag]; + } + + bool walkable = true; + for (int i = resCount; i < result.Count; i++) { + penaltySum2 += result[i].Penalty + tagPenalties[result[i].Tag]; + walkable &= ((traversableTags >> (int)result[i].Tag) & 1) == 1; + } + + // Allow 40% more penalty on average per node + if (!walkable || (penaltySum*1.4*(result.Count-resCount)) < (penaltySum2*(mn-start+1)) || result[result.Count-1] != nodes[mn]) { + // Linecast hit the wrong node or it is a lot more expensive than the original path + result.RemoveRange(resCount, result.Count-resCount); + + result.Add(nodes[start]); + start++; + } else { + //Remove nodes[end] + result.RemoveAt(result.Count-1); + start = mn; + } + } + } + } + + public static FunnelPortals ConstructFunnelPortals (List nodes, PathPart part) { + if (nodes == null || nodes.Count == 0) { + return new FunnelPortals { left = ListPool.Claim(0), right = ListPool.Claim(0) }; + } + + if (part.endIndex < part.startIndex || part.startIndex < 0 || part.endIndex > nodes.Count) throw new System.ArgumentOutOfRangeException(); + + // Claim temporary lists and try to find lists with a high capacity + var left = ListPool.Claim(nodes.Count+1); + var right = ListPool.Claim(nodes.Count+1); + + // Add start point + left.Add(part.startPoint); + right.Add(part.startPoint); + + // Loop through all nodes in the path (except the last one) + for (int i = part.startIndex; i < part.endIndex; i++) { + // Get the portal between path[i] and path[i+1] and add it to the left and right lists + if (nodes[i].GetPortal(nodes[i+1], out var lp, out var rp)) { + left.Add(lp); + right.Add(rp); + } else { + // Fallback, just use the positions of the nodes + left.Add((Vector3)nodes[i].position); + right.Add((Vector3)nodes[i].position); + + left.Add((Vector3)nodes[i+1].position); + right.Add((Vector3)nodes[i+1].position); + } + } + + // Add end point + left.Add(part.endPoint); + right.Add(part.endPoint); + + return new FunnelPortals { left = left, right = right }; + } + + [BurstCompile] + public struct FunnelState { + /// Left side of the funnel + public NativeCircularBuffer leftFunnel; + /// Right side of the funnel + public NativeCircularBuffer rightFunnel; + /// + /// Unwrapped version of the funnel portals in 2D space. + /// + /// The input is a funnel like in the image below. It may be rotated and twisted. + /// [Open online documentation to see images] + /// The output will be a funnel in 2D space like in the image below. All twists and bends will have been straightened out. + /// [Open online documentation to see images] + /// + /// This array is used as a cache and the unwrapped portals are calculated on demand. Thus it may not contain all portals. + /// + public NativeCircularBuffer unwrappedPortals; + + /// + /// If set to anything other than (0,0,0), then all portals will be projected on a plane with this normal. + /// + /// This is used to make the funnel fit a rotated graph better. + /// It is ideally used for grid graphs, but navmesh/recast graphs are probably better off with it set to zero. + /// + /// The vector should be normalized (unless zero), in world space, and should never be changed after the first portal has been added (unless the funnel is cleared first). + /// + public float3 projectionAxis; + + + public FunnelState(int initialCapacity, Allocator allocator) { + leftFunnel = new NativeCircularBuffer(initialCapacity, allocator); + rightFunnel = new NativeCircularBuffer(initialCapacity, allocator); + unwrappedPortals = new NativeCircularBuffer(initialCapacity, allocator); + projectionAxis = float3.zero; + } + + public FunnelState(FunnelPortals portals, Allocator allocator) : this(portals.left.Count, allocator) { + if (portals.left.Count != portals.right.Count) throw new System.ArgumentException("portals.left.Count != portals.right.Count"); + for (int i = 0; i < portals.left.Count; i++) { + PushEnd(portals.left[i], portals.right[i]); + } + } + + public FunnelState Clone () { + return new FunnelState { + leftFunnel = leftFunnel.Clone(), + rightFunnel = rightFunnel.Clone(), + unwrappedPortals = unwrappedPortals.Clone(), + projectionAxis = projectionAxis, + }; + } + + public void Clear () { + leftFunnel.Clear(); + rightFunnel.Clear(); + unwrappedPortals.Clear(); + projectionAxis = float3.zero; + } + + public void PopStart () { + leftFunnel.PopStart(); + rightFunnel.PopStart(); + if (unwrappedPortals.Length > 0) unwrappedPortals.PopStart(); + } + + public void PopEnd () { + leftFunnel.PopEnd(); + rightFunnel.PopEnd(); + unwrappedPortals.TrimTo(leftFunnel.Length); + } + + public void Pop (bool fromStart) { + if (fromStart) PopStart(); + else PopEnd(); + } + + public void PushStart (float3 newLeftPortal, float3 newRightPortal) { + PushStart(ref leftFunnel, ref rightFunnel, ref unwrappedPortals, ref newLeftPortal, ref newRightPortal, ref projectionAxis); + } + + /// True if a and b lie on different sides of the infinite line that passes through start and end + static bool DifferentSidesOfLine (float3 start, float3 end, float3 a, float3 b) { + var portal = math.normalizesafe(end - start); + var d1 = a - start; + var d2 = b - start; + d1 -= portal * math.dot(d1, portal); + d2 -= portal * math.dot(d2, portal); + return math.dot(d1, d2) < 0; + } + + /// + /// True if it is reasonable that the given start point has passed the first portal in the funnel. + /// + /// If this is true, it is most likely better to pop the start/end portal of the funnel first. + /// + /// This can be used as a heuristic to determine if the agent has passed a portal and we should pop it, + /// in those cases when node information is not available (e.g. because the path has been invalidated). + /// + public bool IsReasonableToPopStart (float3 startPoint, float3 endPoint) { + if (leftFunnel.Length == 0) return false; + + var reference = 1; + while (reference < leftFunnel.Length && VectorMath.IsColinear(leftFunnel.First, rightFunnel.First, leftFunnel[reference])) { + reference++; + } + return !DifferentSidesOfLine(leftFunnel.First, rightFunnel.First, startPoint, reference < leftFunnel.Length ? leftFunnel[reference] : endPoint); + } + + /// Like but for the end of the funnel + public bool IsReasonableToPopEnd (float3 startPoint, float3 endPoint) { + if (leftFunnel.Length == 0) return false; + + var reference = leftFunnel.Length - 1; + while (reference >= 0 && VectorMath.IsColinear(leftFunnel.Last, rightFunnel.Last, leftFunnel[reference])) { + reference--; + } + return !DifferentSidesOfLine(leftFunnel.Last, rightFunnel.Last, endPoint, reference >= 0 ? leftFunnel[reference] : startPoint); + } + + [BurstCompile] + static void PushStart (ref NativeCircularBuffer leftPortals, ref NativeCircularBuffer rightPortals, ref NativeCircularBuffer unwrappedPortals, ref float3 newLeftPortal, ref float3 newRightPortal, ref float3 projectionAxis) { + if (unwrappedPortals.Length == 0) { + leftPortals.PushStart(newLeftPortal); + rightPortals.PushStart(newRightPortal); + return; + } + + var firstUnwrapped = unwrappedPortals.First; + var unwrappedRight = Unwrap(leftPortals.First, rightPortals.First, firstUnwrapped.xy, firstUnwrapped.zw, newRightPortal, -1, projectionAxis); + var unwrappedLeft = Unwrap(leftPortals.First, newRightPortal, firstUnwrapped.xy, unwrappedRight, newLeftPortal, -1, projectionAxis); + leftPortals.PushStart(newLeftPortal); + rightPortals.PushStart(newRightPortal); + unwrappedPortals.PushStart(new float4(unwrappedLeft, unwrappedRight)); + } + + public void Splice (int startIndex, int toRemove, List newLeftPortal, List newRightPortal) { + this.leftFunnel.Splice(startIndex, toRemove, newLeftPortal); + this.rightFunnel.Splice(startIndex, toRemove, newRightPortal); + unwrappedPortals.TrimTo(startIndex); + } + + public void PushEnd (Vector3 newLeftPortal, Vector3 newRightPortal) { + leftFunnel.PushEnd(newLeftPortal); + rightFunnel.PushEnd(newRightPortal); + } + + public void Push (bool toStart, Vector3 newLeftPortal, Vector3 newRightPortal) { + if (toStart) PushStart(newLeftPortal, newRightPortal); + else PushEnd(newLeftPortal, newRightPortal); + } + + public void Dispose () { + leftFunnel.Dispose(); + rightFunnel.Dispose(); + unwrappedPortals.Dispose(); + } + + /// + /// Calculate the shortest path through the funnel. + /// + /// Returns: The number of corners added to the result array. + /// + /// See: http://digestingduck.blogspot.se/2010/03/simple-stupid-funnel-algorithm.html + /// + /// The maximum number of corners to add to the result array. Should be positive. + /// Output indices. Contains an index as well as possibly the \reflink{RightSideBit} set. Corresponds to an index of the left or right portals, depending on if \reflink{RightSideBit} is set. This must point to an array which is at least maxCorners long. + /// Start point of the funnel. The agent will move from here to the best point along the first portal. + /// End point of the funnel. + /// True if the final corner of the path was reached. If true, then the return value is guaranteed to be at most maxCorners - 1 (unless maxCorners = 0). + public int CalculateNextCornerIndices (int maxCorners, NativeArray result, float3 startPoint, float3 endPoint, out bool lastCorner) { + Assert.AreEqual(leftFunnel.Length, rightFunnel.Length); + Assert.IsTrue(unwrappedPortals.Length <= leftFunnel.Length); + if (result.Length < math.min(maxCorners, leftFunnel.Length)) throw new System.ArgumentException("result array may not be large enough to hold all corners"); + + unsafe { + // TODO: Pass this as ref instead? + var resultsSpan = result.AsUnsafeSpan(); + return Calculate(ref unwrappedPortals, ref leftFunnel, ref rightFunnel, ref startPoint, ref endPoint, ref resultsSpan, maxCorners, ref projectionAxis, out lastCorner); + } + } + + public void CalculateNextCorners (int maxCorners, bool splitAtEveryPortal, float3 startPoint, float3 endPoint, NativeList result) { + var indices = new NativeArray(math.min(maxCorners, leftFunnel.Length), Allocator.Temp); + var numCorners = CalculateNextCornerIndices(maxCorners, indices, startPoint, endPoint, out bool lastCorner); + ConvertCornerIndicesToPath(indices, numCorners, splitAtEveryPortal, startPoint, endPoint, lastCorner, result); + indices.Dispose(); + } + + public void ConvertCornerIndicesToPath (NativeArray indices, int numCorners, bool splitAtEveryPortal, float3 startPoint, float3 endPoint, bool lastCorner, NativeList result) { + if (result.Capacity < numCorners) result.Capacity = numCorners; + + Assert.IsTrue(numCorners == 0 || (indices[numCorners-1] & FunnelPortalIndexMask) < unwrappedPortals.Length); + result.Add(startPoint); + if (leftFunnel.Length == 0) { + if (lastCorner) result.Add(endPoint); + return; + } + + if (splitAtEveryPortal) { + float2 prev2D = Unwrap(leftFunnel[0], rightFunnel[0], unwrappedPortals[0].xy, unwrappedPortals[0].zw, startPoint, -1, projectionAxis); + var prevIdx = 0; + for (int i = 0; i < numCorners; i++) { + var idx = indices[i] & FunnelPortalIndexMask; + var rightSide = (indices[i] & RightSideBit) != 0; + // Check intersections with every portal segment + float2 next2D = rightSide ? unwrappedPortals[idx].zw : unwrappedPortals[idx].xy; + CalculatePortalIntersections(prevIdx + 1, idx - 1, leftFunnel, rightFunnel, unwrappedPortals, prev2D, next2D, result); + prevIdx = math.abs(idx); + prev2D = next2D; + + result.Add(rightSide ? rightFunnel[idx] : leftFunnel[idx]); + } + if (lastCorner) { + var next2D = Unwrap(leftFunnel.Last, rightFunnel.Last, unwrappedPortals.Last.xy, unwrappedPortals.Last.zw, endPoint, 1, projectionAxis); + CalculatePortalIntersections(prevIdx + 1, unwrappedPortals.Length - 1, leftFunnel, rightFunnel, unwrappedPortals, prev2D, next2D, result); + result.Add(endPoint); + } + } else { + for (int i = 0; i < numCorners; i++) { + var idx = indices[i]; + result.Add((idx & RightSideBit) != 0 ? rightFunnel[idx & FunnelPortalIndexMask] : leftFunnel[idx & FunnelPortalIndexMask]); + } + if (lastCorner) result.Add(endPoint); + } + } + + public void ConvertCornerIndicesToPathProjected (UnsafeSpan indices, bool splitAtEveryPortal, float3 startPoint, float3 endPoint, bool lastCorner, NativeList result, float3 up) { + var resultCount = indices.Length + 1 + (lastCorner ? 1 : 0); + if (result.Capacity < resultCount) result.Capacity = resultCount; + result.ResizeUninitialized(resultCount); + var resultSpan = result.AsUnsafeSpan(); + ConvertCornerIndicesToPathProjected(ref this, ref indices, splitAtEveryPortal, in startPoint, in endPoint, lastCorner, in projectionAxis, ref resultSpan, in up); + } + + public float4x3 UnwrappedPortalsToWorldMatrix (float3 up) { + int startIndex = 0; + while (startIndex < unwrappedPortals.Length && math.lengthsq(unwrappedPortals[startIndex].xy - unwrappedPortals[startIndex].zw) <= 0.00001f) startIndex++; + if (startIndex >= unwrappedPortals.Length) return new float4x3(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1); + var left2D = unwrappedPortals[startIndex].xy; + var right2D = unwrappedPortals[startIndex].zw; + var left3D = leftFunnel[startIndex]; + var right3D = rightFunnel[startIndex]; + var portal2D = right2D - left2D; + var portal3D = right3D - left3D; + var portal2DInv = portal2D * math.rcp(math.lengthsq(portal2D)); + // Matrix to rotate unwrapped portals so that portal2D maps to the x-axis (1,0) + var mr = new float2x2( + new float2(portal2DInv.x, -portal2DInv.y), + new float2(portal2DInv.y, portal2DInv.x) + ); + + // Matrix to transform points in unwrapped-portal-space so left2D maps to (0,0) and right2D maps to (1,0) + var offset = math.mul(mr, -left2D); + var m1 = new float4x3( + new float4(mr.c0.x, 0, mr.c0.y, 0), + new float4(mr.c1.x, 0, mr.c1.y, 0), + new float4(offset.x, 0, offset.y, 1) + ); + + // Matrix that maps (0,0,0) to left3D and (1,0,0) to right3D, as well as (0,1,0) to up. + var m2 = new float4x4( + new float4(portal3D, 0), + new float4(up, 0), + new float4(math.cross(portal3D, up), 0), + new float4(left3D, 1) + ); + + // Matrix to transform points in unwrapped-portal-space to 3D space. Such that left2D maps to left3D. + return math.mul(m2, m1); + } + + [BurstCompile] + public static void ConvertCornerIndicesToPathProjected (ref FunnelState funnelState, ref UnsafeSpan indices, bool splitAtEveryPortal, in float3 startPoint, in float3 endPoint, bool lastCorner, in float3 projectionAxis, ref UnsafeSpan result, in float3 up) { + Assert.IsTrue(indices.Length == 0 || (indices[indices.Length-1] & FunnelPortalIndexMask) < funnelState.unwrappedPortals.Length); + int resultIndex = 0; + result[resultIndex++] = startPoint; + if (funnelState.leftFunnel.Length == 0) { + if (lastCorner) result[resultIndex++] = endPoint; + Assert.AreEqual(resultIndex, result.Length); + return; + } + + var unwrappedToWorld = funnelState.UnwrappedPortalsToWorldMatrix(up); + + if (splitAtEveryPortal) { + throw new System.NotImplementedException(); + } else { + for (int i = 0; i < indices.Length; i++) { + var idx = indices[i]; + var corner = (idx & RightSideBit) != 0 ? funnelState.unwrappedPortals[idx & FunnelPortalIndexMask].zw : funnelState.unwrappedPortals[idx & FunnelPortalIndexMask].xy; + result[resultIndex++] = math.mul(unwrappedToWorld, new float3(corner, 1)).xyz; + } + if (lastCorner) { + float2 endPoint2D = Unwrap(funnelState.leftFunnel.Last, funnelState.rightFunnel.Last, funnelState.unwrappedPortals.Last.xy, funnelState.unwrappedPortals.Last.zw, endPoint, 1, projectionAxis); + result[resultIndex++] = math.mul(unwrappedToWorld, new float3(endPoint2D, 1)).xyz; + } + } + Assert.AreEqual(resultIndex, result.Length); + } + + static void CalculatePortalIntersections (int startIndex, int endIndex, NativeCircularBuffer leftPortals, NativeCircularBuffer rightPortals, NativeCircularBuffer unwrappedPortals, float2 from, float2 to, NativeList result) { + for (int j = startIndex; j < endIndex; j++) { + var portal = unwrappedPortals[j]; + var left = portal.xy; + var right = portal.zw; + if (!VectorMath.LineLineIntersectionFactor(left, right - left, from, to - from, out float factor)) { + // This really shouldn't happen + factor = 0.5f; + } + result.Add(math.lerp(leftPortals[j], rightPortals[j], factor)); + } + } + } + + private static float2 Unwrap (float3 leftPortal, float3 rightPortal, float2 leftUnwrappedPortal, float2 rightUnwrappedPortal, float3 point, float sideMultiplier, float3 projectionAxis) { + // TODO: On grid graphs this is kind of a weird way to do it. + // We project all points onto a plane and then unwrap them. + // It would be faster (and possibly more numerically accurate) to transform the points to graph space and then just use the xz coordinates. + // We also slow down the navmesh case by adding these 3 dot products. + leftPortal -= math.dot(leftPortal, projectionAxis); + rightPortal -= math.dot(rightPortal, projectionAxis); + point -= math.dot(point, projectionAxis); + + var portal = rightPortal - leftPortal; + var portalLengthInvSq = 1.0f / math.lengthsq(portal); + if (float.IsPositiveInfinity(portalLengthInvSq)) { + return leftUnwrappedPortal + new float2(-math.length(point - leftPortal), 0); + } + var distance = math.length(math.cross(point - leftPortal, portal)) * portalLengthInvSq; + var projection = math.dot(point - leftPortal, portal) * portalLengthInvSq; + + // Weld corner vertices if they are close enough. + // This is important for grid graphs, as if the unwrapped portals are not quite identical in the corners, + // the grid simplification may fail to remove inner corners. This is because it will detect 2+ almost identical corners in each turn, instead of 1. + // TODO: Unwrap grid portals in a different way. It really can be done much faster and more numerically robustly. + // We should not use graph space directly, though, as grid graphs can move around (ProceduralGraphMover). + if (distance < 0.002f) { + if (math.abs(projection) < 0.002f) { + return leftUnwrappedPortal; + } else if (math.abs(projection - 1) < 0.002f) { + return rightUnwrappedPortal; + } + } + + var unwrappedPortal = rightUnwrappedPortal - leftUnwrappedPortal; + var unwrappedNormal = new float2(-unwrappedPortal.y, unwrappedPortal.x); + return leftUnwrappedPortal + math.mad(unwrappedPortal, projection, unwrappedNormal * (distance * sideMultiplier)); + } + + /// True if b is to the right of or on the line from (0,0) to a + private static bool RightOrColinear (Vector2 a, Vector2 b) { + return (a.x*b.y - b.x*a.y) <= 0; + } + + /// True if b is to the left of or on the line from (0,0) to a + private static bool LeftOrColinear (Vector2 a, Vector2 b) { + return (a.x*b.y - b.x*a.y) >= 0; + } + + /// + /// Calculate the shortest path through the funnel. + /// + /// The path will be unwrapped into 2D space before the funnel algorithm runs. + /// This makes it possible to support the funnel algorithm in XY space as well as in more complicated cases, such as on curved worlds. + /// [Open online documentation to see images] + /// + /// [Open online documentation to see images] + /// + /// See: Unwrap + /// + /// The portals of the funnel. The first and last vertices portals must be single points (so for example left[0] == right[0]). + /// If true, then a vertex will be inserted every time the path crosses a portal + /// instead of only at the corners of the path. The result will have exactly one vertex per portal if this is enabled. + /// This may introduce vertices with the same position in the output (esp. in corners where many portals meet). + public static List Calculate (FunnelPortals funnel, bool splitAtEveryPortal) { + var state = new FunnelState(funnel, Allocator.Temp); + var startPoint = state.leftFunnel.First; + var endPoint = state.leftFunnel.Last; + state.PopStart(); + state.PopEnd(); + var nativeResult = new NativeList(Allocator.Temp); + state.CalculateNextCorners(int.MaxValue, splitAtEveryPortal, startPoint, endPoint, nativeResult); + state.Dispose(); + var result = ListPool.Claim(nativeResult.Length); + for (int i = 0; i < nativeResult.Length; i++) result.Add((Vector3)nativeResult[i]); + nativeResult.Dispose(); + return result; + } + + public const int RightSideBit = 1 << 30; + public const int FunnelPortalIndexMask = RightSideBit - 1; + + /// + /// Calculate the shortest path through the funnel. + /// + /// Returns: The number of corners added to the funnelPath array. + /// + /// See: http://digestingduck.blogspot.se/2010/03/simple-stupid-funnel-algorithm.html + /// + /// Left side of the funnel. Should not contain the start point. + /// Right side of the funnel. Should not contain the end point. + /// Cache of unwrapped portal segments. This may be empty, but it will be filled with unwrapped portals and next time you run the algorithm it will be faster. + /// Start point of the funnel. The agent will move from here to the best point between leftPortals[0] and rightPortals[0]. + /// End point of the funnel. + /// Output indices. Contains an index as well as possibly the \reflink{RightSideBit} set. Corresponds to an index into leftPortals or rightPortals depending on if \reflink{RightSideBit} is set. This must point to an array which is at least maxCorners long. + /// True if the final corner of the path was reached. If true, then the return value is guaranteed to be at most maxCorners - 1 (unless maxCorners = 0). + /// The first N corners of the optimized path will be calculated. Calculating fewer corners is faster. Pass int.MaxValue if you want to calculate all corners. + /// If set to anything other than (0,0,0), then all portals will be projected on a plane with this normal. + [BurstCompile] + static unsafe int Calculate (ref NativeCircularBuffer unwrappedPortals, ref NativeCircularBuffer leftPortals, ref NativeCircularBuffer rightPortals, ref float3 startPoint, ref float3 endPoint, ref UnsafeSpan funnelPath, int maxCorners, ref float3 projectionAxis, out bool lastCorner) { + lastCorner = false; + if (leftPortals.Length <= 0) { + lastCorner = true; + return 0; + } + if (maxCorners <= 0) return 0; + + int apexIndex = 0; + int rightIndex = 0; + int leftIndex = 0; + + int outputCount = 0; + + if (unwrappedPortals.Length == 0) { + unwrappedPortals.PushEnd(new float4(new float2(0, 0), new float2(math.length(rightPortals[0] - leftPortals[0])))); + } + + float2 portalApex = Unwrap(leftPortals[0], rightPortals[0], unwrappedPortals[0].xy, unwrappedPortals[0].zw, startPoint, -1, projectionAxis); + float2 portalLeft = float2.zero; + float2 portalRight = float2.zero; + + for (int i = 0; i <= leftPortals.Length; i++) { + // Unwrap the funnel on the fly as needed + float2 rLeft, rRight; + if (i == unwrappedPortals.Length) { + if (i == leftPortals.Length) { + // The end point of the path + rLeft = rRight = Unwrap(leftPortals[i-1], rightPortals[i-1], unwrappedPortals[i-1].xy, unwrappedPortals[i-1].zw, endPoint, 1, projectionAxis) - portalApex; + } else { + // The funnel needs unwrapping + var unwrappedLeft = Unwrap(leftPortals[i-1], rightPortals[i-1], unwrappedPortals[i-1].xy, unwrappedPortals[i-1].zw, leftPortals[i], 1, projectionAxis); + var unwrappedRight = Unwrap(leftPortals[i], rightPortals[i-1], unwrappedLeft, unwrappedPortals[i-1].zw, rightPortals[i], 1, projectionAxis); + unwrappedPortals.PushEnd(new float4(unwrappedLeft, unwrappedRight)); + rLeft = unwrappedLeft - portalApex; + rRight = unwrappedRight - portalApex; + } + } else { + // Common case + rLeft = unwrappedPortals[i].xy - portalApex; + rRight = unwrappedPortals[i].zw - portalApex; + } + + if (LeftOrColinear(portalRight, rRight)) { + if (RightOrColinear(portalLeft, rRight)) { + portalRight = rRight; + rightIndex = i; + } else { + portalRight = portalLeft = float2.zero; + i = apexIndex = rightIndex = leftIndex; + portalApex = unwrappedPortals[i].xy; + + funnelPath[outputCount++] = apexIndex; + if (outputCount >= maxCorners) return outputCount; + continue; + } + } + + if (RightOrColinear(portalLeft, rLeft)) { + if (LeftOrColinear(portalRight, rLeft)) { + portalLeft = rLeft; + leftIndex = i; + } else { + portalRight = portalLeft = float2.zero; + i = apexIndex = leftIndex = rightIndex; + portalApex = unwrappedPortals[i].zw; + + funnelPath[outputCount++] = apexIndex | RightSideBit; + if (outputCount >= maxCorners) return outputCount; + continue; + } + } + } + + lastCorner = true; + return outputCount; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Funnel.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Funnel.cs.meta new file mode 100644 index 0000000..dfb8b16 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Funnel.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 332338376eb42424caa0e3d6bf984a8b +timeCreated: 1488237337 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs new file mode 100644 index 0000000..ad14f12 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs @@ -0,0 +1,247 @@ +using UnityEngine; + +namespace Pathfinding.Util { + using Pathfinding.Drawing; + + /// Combines hashes into a single hash value + public struct NodeHasher { + readonly bool includePathSearchInfo; + readonly bool includeAreaInfo; + readonly bool includeHierarchicalNodeInfo; + readonly PathHandler debugData; + public DrawingData.Hasher hasher; + + public NodeHasher(AstarPath active) { + hasher = default; + this.debugData = active.debugPathData; + includePathSearchInfo = debugData != null && (active.debugMode == GraphDebugMode.F || active.debugMode == GraphDebugMode.G || active.debugMode == GraphDebugMode.H || active.showSearchTree); + includeAreaInfo = active.debugMode == GraphDebugMode.Areas; + includeHierarchicalNodeInfo = active.debugMode == GraphDebugMode.HierarchicalNode; + hasher.Add(active.debugMode); + hasher.Add(active.debugFloor); + hasher.Add(active.debugRoof); + hasher.Add(AstarColor.ColorHash()); + } + + public void HashNode (GraphNode node) { + hasher.Add(node.GetGizmoHashCode()); + if (includeAreaInfo) hasher.Add((int)node.Area); + if (includeHierarchicalNodeInfo) hasher.Add(node.HierarchicalNodeIndex); + + if (includePathSearchInfo) { + var pathNode = debugData.pathNodes[node.NodeIndex]; + hasher.Add(pathNode.pathID); + hasher.Add(pathNode.pathID == debugData.PathID); + // hasher.Add(pathNode.F); + } + } + + public void Add(T v) { + hasher.Add(v); + } + + public static implicit operator DrawingData.Hasher(NodeHasher hasher) { + return hasher.hasher; + } + } + + public class GraphGizmoHelper : IAstarPooledObject, System.IDisposable { + public DrawingData.Hasher hasher { get; private set; } + PathHandler debugData; + ushort debugPathID; + GraphDebugMode debugMode; + bool showSearchTree; + float debugFloor; + float debugRoof; + public CommandBuilder builder; + Vector3 drawConnectionStart; + Color drawConnectionColor; + readonly System.Action drawConnection; +#if UNITY_EDITOR + UnsafeSpan debugPathNodes; +#endif + GlobalNodeStorage nodeStorage; + + public GraphGizmoHelper () { + // Cache a delegate to avoid allocating memory for it every time + drawConnection = DrawConnection; + } + + public static GraphGizmoHelper GetSingleFrameGizmoHelper (DrawingData gizmos, AstarPath active, RedrawScope redrawScope) { + return GetGizmoHelper(gizmos, active, DrawingData.Hasher.NotSupplied, redrawScope); + } + + public static GraphGizmoHelper GetGizmoHelper (DrawingData gizmos, AstarPath active, DrawingData.Hasher hasher, RedrawScope redrawScope) { + var helper = ObjectPool.Claim(); + + helper.Init(active, hasher, gizmos, redrawScope); + return helper; + } + + public void Init (AstarPath active, DrawingData.Hasher hasher, DrawingData gizmos, RedrawScope redrawScope) { + if (active != null) { + debugData = active.debugPathData; + debugPathID = active.debugPathID; + debugMode = active.debugMode; + debugFloor = active.debugFloor; + debugRoof = active.debugRoof; + nodeStorage = active.nodeStorage; +#if UNITY_EDITOR + if (debugData != null && debugData.threadID < active.nodeStorage.pathfindingThreadData.Length) debugPathNodes = active.nodeStorage.pathfindingThreadData[debugData.threadID].debugPathNodes; + else debugPathNodes = default; + showSearchTree = active.showSearchTree && debugPathNodes.Length > 0; +#else + showSearchTree = false; +#endif + } + this.hasher = hasher; + builder = gizmos.GetBuilder(hasher, redrawScope); + } + + public void OnEnterPool () { + builder.Dispose(); + debugData = null; + } + + public void DrawConnections (GraphNode node) { + if (showSearchTree) { +#if UNITY_EDITOR + if (debugPathNodes.Length > 0) { + var nodeIndex = node.NodeIndex; + for (uint i = 0; i < (uint)node.PathNodeVariants; i++) { + var pnode = debugPathNodes[nodeIndex + i]; + if (pnode.pathID == debugPathID) { + if (pnode.parentIndex != 0 && debugPathNodes[pnode.parentIndex].pathID == debugPathID) { + var parent = nodeStorage.GetNode(pnode.parentIndex); + if (parent != null) { + var nodePos = node.DecodeVariantPosition(nodeIndex + i, pnode.fractionAlongEdge); + var parentPos = parent.DecodeVariantPosition(pnode.parentIndex, debugPathNodes[pnode.parentIndex].fractionAlongEdge); + builder.Line((Vector3)parentPos, (Vector3)nodePos, NodeColor(node)); + } + } + } + } + } +#endif + } else { + // Calculate which color to use for drawing the node + // based on the settings specified in the editor + drawConnectionColor = NodeColor(node); + // Get the node position + // Cast it here to avoid doing it for every neighbour + drawConnectionStart = (Vector3)node.position; + node.GetConnections(drawConnection); + } + } + + void DrawConnection (GraphNode other) { + builder.Line(drawConnectionStart, ((Vector3)other.position + drawConnectionStart)*0.5f, drawConnectionColor); + } + + /// + /// Color to use for gizmos. + /// Returns a color to be used for the specified node with the current debug settings (editor only). + /// + /// Version: Since 3.6.1 this method will not handle null nodes + /// + public Color NodeColor (GraphNode node) { +#if UNITY_EDITOR + if (showSearchTree && !InSearchTree(node, debugPathNodes, debugPathID)) return Color.clear; +#endif + + Color color; + + if (node.Walkable) { + switch (debugMode) { + case GraphDebugMode.Areas: + color = AstarColor.GetAreaColor(node.Area); + break; + case GraphDebugMode.HierarchicalNode: + case GraphDebugMode.NavmeshBorderObstacles: + color = AstarColor.GetTagColor((uint)node.HierarchicalNodeIndex); + break; + case GraphDebugMode.Penalty: + color = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, ((float)node.Penalty-debugFloor) / (debugRoof-debugFloor)); + break; + case GraphDebugMode.Tags: + color = AstarColor.GetTagColor(node.Tag); + break; + case GraphDebugMode.SolidColor: + color = AstarColor.SolidColor; + break; + default: +#if UNITY_EDITOR + if (debugPathNodes.Length == 0) { + color = AstarColor.SolidColor; + break; + } + + var pathNode = debugPathNodes[node.NodeIndex]; + float value; + if (debugMode == GraphDebugMode.G) { + value = pathNode.g; + } else if (debugMode == GraphDebugMode.H) { + value = pathNode.h; + } else { + // mode == F + value = pathNode.g + pathNode.h; + } + + color = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (value-debugFloor) / (debugRoof-debugFloor)); +#else + color = AstarColor.SolidColor; +#endif + break; + } + } else { + color = AstarColor.UnwalkableNode; + } + + return color; + } + +#if UNITY_EDITOR + /// + /// Returns if the node is in the search tree of the path. + /// Only guaranteed to be correct if path is the latest path calculated. + /// Use for gizmo drawing only. + /// + internal static bool InSearchTree (GraphNode node, UnsafeSpan debugPathNodes, ushort pathID) { + if (debugPathNodes.Length > 0) { + for (uint i = 0; i < node.PathNodeVariants; i++) { + if (debugPathNodes[node.NodeIndex + i].pathID == pathID) { + return true; + } + } + } + return false; + } +#endif + + public void DrawWireTriangle (Vector3 a, Vector3 b, Vector3 c, Color color) { + builder.Line(a, b, color); + builder.Line(b, c, color); + builder.Line(c, a, color); + } + + public void DrawTriangles (Vector3[] vertices, Color[] colors, int numTriangles) { + var triangles = ArrayPool.Claim(numTriangles*3); + + for (int i = 0; i < numTriangles*3; i++) triangles[i] = i; + builder.SolidMesh(vertices, triangles, colors, numTriangles*3, numTriangles*3); + ArrayPool.Release(ref triangles); + } + + public void DrawWireTriangles (Vector3[] vertices, Color[] colors, int numTriangles) { + for (int i = 0; i < numTriangles; i++) { + DrawWireTriangle(vertices[i*3+0], vertices[i*3+1], vertices[i*3+2], colors[i*3+0]); + } + } + + void System.IDisposable.Dispose () { + var tmp = this; + + ObjectPool.Release(ref tmp); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs.meta new file mode 100644 index 0000000..5994041 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 20ed0bf48b2a14d78b8aa2ebe33f0069 +timeCreated: 1473875958 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphUpdateUtilities.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphUpdateUtilities.cs new file mode 100644 index 0000000..a70c5eb --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphUpdateUtilities.cs @@ -0,0 +1,104 @@ +using System.Collections.Generic; +using Pathfinding.Util; + +namespace Pathfinding { + /// + /// Contains useful functions for updating graphs. + /// This class works a lot with the GraphNode class, a useful function to get nodes is . + /// + /// See: + /// See: + /// + public static class GraphUpdateUtilities { + /// + /// Updates graphs and checks if all nodes are still reachable from each other. + /// Graphs are updated, then a check is made to see if the nodes are still reachable from each other. + /// If they are not, the graphs are reverted to before the update and false is returned. + /// This is slower than a normal graph update. + /// All queued graph updates and thread safe callbacks will be flushed during this function. + /// + /// Returns: True if the given nodes are still reachable from each other after the guo has been applied. False otherwise. + /// + /// + /// var guo = new GraphUpdateObject(tower.GetComponent().bounds); + /// var spawnPointNode = AstarPath.active.GetNearest(spawnPoint.position).node; + /// var goalNode = AstarPath.active.GetNearest(goalPoint.position).node; + /// + /// if (GraphUpdateUtilities.UpdateGraphsNoBlock(guo, spawnPointNode, goalNode, false)) { + /// // Valid tower position + /// // Since the last parameter (which is called "alwaysRevert") in the method call was false + /// // The graph is now updated and the game can just continue + /// } else { + /// // Invalid tower position. It blocks the path between the spawn point and the goal + /// // The effect on the graph has been reverted + /// Destroy(tower); + /// } + /// + /// + /// Warning: This will not work for recast graphs if is enabled (the default). + /// + /// The GraphUpdateObject to update the graphs with + /// Node which should have a valid path to node2. All nodes should be walkable or false will be returned. + /// Node which should have a valid path to node1. All nodes should be walkable or false will be returned. + /// If true, reverts the graphs to the old state even if no blocking occurred + public static bool UpdateGraphsNoBlock (GraphUpdateObject guo, GraphNode node1, GraphNode node2, bool alwaysRevert = false) { + List buffer = ListPool.Claim(); + + buffer.Add(node1); + buffer.Add(node2); + + bool worked = UpdateGraphsNoBlock(guo, buffer, alwaysRevert); + ListPool.Release(ref buffer); + return worked; + } + + /// + /// Updates graphs and checks if all nodes are still reachable from each other. + /// Graphs are updated, then a check is made to see if the nodes are still reachable from each other. + /// If they are not, the graphs are reverted to before the update and false is returned. + /// This is slower than a normal graph update. + /// All queued graph updates will be flushed during this function. + /// + /// Returns: True if the given nodes are still reachable from each other after the guo has been applied. False otherwise. + /// + /// The GraphUpdateObject to update the graphs with + /// Nodes which should have valid paths between them. All nodes should be walkable or false will be returned. + /// If true, reverts the graphs to the old state even if no blocking occurred + public static bool UpdateGraphsNoBlock (GraphUpdateObject guo, List nodes, bool alwaysRevert = false) { + bool worked; + + // Pause pathfinding while modifying the graphs + var graphLock = AstarPath.active.PausePathfinding(); + + try { + // Make sure any pending graph updates have been done before we start + AstarPath.active.FlushGraphUpdates(); + + // Make sure all nodes are walkable + for (int i = 0; i < nodes.Count; i++) if (!nodes[i].Walkable) return false; + + // Create a snapshot to allow us to revert the graphs to their original state + var snapshot = AstarPath.active.Snapshot(guo.bounds, guo.nnConstraint.graphMask); + + AstarPath.active.UpdateGraphs(guo); + + // Update the graphs immediately + AstarPath.active.FlushGraphUpdates(); + + // Check if all nodes are in the same area and that they are walkable, i.e that there are paths between all of them + worked = PathUtilities.IsPathPossible(nodes); + + // If it did not work, revert the GUO + if (!worked || alwaysRevert) { + AstarPath.active.AddWorkItem(snapshot.Restore); + AstarPath.active.FlushWorkItems(); + } + snapshot.Dispose(); + } finally { + graphLock.Release(); + } + + return worked; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphUpdateUtilities.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphUpdateUtilities.cs.meta new file mode 100644 index 0000000..58f3863 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphUpdateUtilities.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: b5818f65b5e1449c1ae6f20de9f75ff5 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs new file mode 100644 index 0000000..fde39e9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs @@ -0,0 +1,577 @@ +using System.Collections.Generic; +using Pathfinding.Drawing; +using Pathfinding.Util; +using Unity.Mathematics; +using UnityEngine; +using UnityEngine.Profiling; + +namespace Pathfinding { + /// + /// Simplifies a path on a grid graph using a string pulling algorithm. + /// This is based on a paper called "Toward a String-Pulling Approach to Path Smoothing on Grid Graphs", + /// with some optimizations as well as fixes for some edge cases that the paper didn't handle. + /// + /// The result is conceptually similar to the well known funnel string pulling algorithm for navmesh graphs + /// but it uses a different algorithm. + /// + /// This class is used by the on grid graphs. + /// + /// See: + /// See: + /// See: article: https://ojs.aaai.org/index.php/SOCS/article/view/18541 + /// + public static class GridStringPulling { + /// + /// Z + /// | + /// | + /// + /// 3 2 + /// \ | / + /// -- - X - ----- X + /// / | \ + /// 0 1 + /// + /// | + /// | + /// + static int2[] directionToCorners = new int2[] { + new int2(0, 0), + new int2(FixedPrecisionScale, 0), + new int2(FixedPrecisionScale, FixedPrecisionScale), + new int2(0, FixedPrecisionScale), + }; + + static long Cross (int2 lhs, int2 rhs) { + return (long)lhs.x*(long)rhs.y - (long)lhs.y*(long)rhs.x; + } + + static long Dot (int2 a, int2 b) { + return (long)a.x*(long)b.x + (long)a.y*(long)b.y; + } + + static bool RightOrColinear (int2 a, int2 b, int2 p) { + return (long)(b.x - a.x) * (long)(p.y - a.y) - (long)(p.x - a.x) * (long)(b.y - a.y) <= 0; + } + + static int2 Perpendicular (int2 v) { + return new int2(-v.y, v.x); + } + + struct TriangleBounds { + int2 d1, d2, d3; + long t1, t2, t3; + + public TriangleBounds(int2 p1, int2 p2, int2 p3) { + if (RightOrColinear(p1, p2, p3)) { + var tmp = p3; + p3 = p1; + p1 = tmp; + } + d1 = Perpendicular(p2 - p1); + d2 = Perpendicular(p3 - p2); + d3 = Perpendicular(p1 - p3); + t1 = Dot(d1, p1); + t2 = Dot(d2, p2); + t3 = Dot(d3, p3); + } + + public bool Contains (int2 p) { + return Dot(d1, p) >= t1 && Dot(d2, p) >= t2 && Dot(d3, p) >= t3; + } + } + + const int FixedPrecisionScale = 1024; + + static int2 ToFixedPrecision (Vector2 p) { + return new int2(math.round(new float2(p)*FixedPrecisionScale)); + } + + static Vector2 FromFixedPrecision (int2 p) { + return (Vector2)(((float2)p) * (1.0f/FixedPrecisionScale)); + } + + /// Returns which side of the line a - b that p lies on + static Side Side2D (int2 a, int2 b, int2 p) { + var s = Cross(b-a, p-a); + + return s > 0 ? Side.Left : (s < 0 ? Side.Right : Side.Colinear); + } + + static Unity.Profiling.ProfilerMarker marker1 = new Unity.Profiling.ProfilerMarker("Linecast hit"); + static Unity.Profiling.ProfilerMarker marker2 = new Unity.Profiling.ProfilerMarker("Linecast success"); + static Unity.Profiling.ProfilerMarker marker3 = new Unity.Profiling.ProfilerMarker("Trace"); + static Unity.Profiling.ProfilerMarker marker4 = new Unity.Profiling.ProfilerMarker("Neighbours"); + static Unity.Profiling.ProfilerMarker marker5 = new Unity.Profiling.ProfilerMarker("Re-evaluate linecast"); + static Unity.Profiling.ProfilerMarker marker6 = new Unity.Profiling.ProfilerMarker("Init"); + static Unity.Profiling.ProfilerMarker marker7 = new Unity.Profiling.ProfilerMarker("Initloop"); + + /// + /// Intersection length of the given segment with a square of size Int3.Precision centered at nodeCenter. + /// The return value is between 0 and sqrt(2). + /// + public static float IntersectionLength (int2 nodeCenter, int2 segmentStart, int2 segmentEnd) { + // TODO: Calculations can be hoisted + var invNormal = math.rcp((float2)(segmentEnd - segmentStart)); + var normalMagn = math.length((float2)(segmentEnd - segmentStart)); + + float tmin = float.NegativeInfinity, tmax = float.PositiveInfinity; + + var normal = segmentEnd - segmentStart; + var bmin = nodeCenter; // - new int2(Int3.Precision/2, Int3.Precision/2); + var bmax = nodeCenter + new int2(FixedPrecisionScale, FixedPrecisionScale); + + if (normal.x != 0.0) { + float tx1 = (bmin.x - segmentStart.x)*invNormal.x; + float tx2 = (bmax.x - segmentStart.x)*invNormal.x; + + tmin = math.max(tmin, math.min(tx1, tx2)); + tmax = math.min(tmax, math.max(tx1, tx2)); + } else if (segmentStart.x < bmin.x || segmentStart.x > bmax.x) { + return 0.0f; + } + + if (normal.y != 0.0) { + float ty1 = (bmin.y - segmentStart.y)*invNormal.y; + float ty2 = (bmax.y - segmentStart.y)*invNormal.y; + + tmin = math.max(tmin, math.min(ty1, ty2)); + tmax = math.min(tmax, math.max(ty1, ty2)); + } else if (segmentStart.y < bmin.y || segmentStart.y > bmax.y) { + return 0.0f; + } + + tmin = math.max(0, tmin); + tmax = math.min(1, tmax); + return math.max(tmax - tmin, 0)*normalMagn*(1.0f/FixedPrecisionScale); + } + + internal static void TestIntersectionLength () { + var s = FixedPrecisionScale; + + UnityEngine.Assertions.Assert.AreApproximatelyEqual(math.sqrt(2), IntersectionLength(new int2(1*s, 1*s), new int2(0, 0), new int2(2*s, 2*s))); + UnityEngine.Assertions.Assert.AreApproximatelyEqual(0.0f, IntersectionLength(new int2(1*s, 1*s), new int2(0, 0), new int2(0, 0))); + UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(-1*s, s+1), new int2(2*s, s+1))); + UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(2*s, s), new int2(-1*s, s))); + + // All sides of the square should be included + UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s, s), new int2(s+s, s))); + UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s+s, s), new int2(s+s, s+s))); + UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s+s, s+s), new int2(s, s+s))); + UnityEngine.Assertions.Assert.AreApproximatelyEqual(1.0f, IntersectionLength(new int2(1*s, 1*s), new int2(s, s+s), new int2(s, s))); + } + + /// + /// Cost of moving across all the nodes in the list, along the given segment. + /// It is assumed that the segment intersects the nodes. Any potentially intersecting nodes that are not part of the list will be ignored. + /// + static uint LinecastCost (List trace, int2 segmentStart, int2 segmentEnd, GridGraph gg, System.Func traversalCost) { + // Check the cost of the segment compared to not taking this "shortcut" + uint cost = 0; + + for (int k = 0; k < trace.Count; k++) { + var node = trace[k] as GridNodeBase; + // Note: this assumes the default grid connection costs are used. Which is relatively reasonable + // since they require changing the code to modify. + cost += (uint)(((float)traversalCost(node) + gg.nodeSize*Int3.Precision) * IntersectionLength(new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid)*FixedPrecisionScale, segmentStart, segmentEnd)); + } + return cost; + } + + enum PredicateFailMode { + Undefined, + Turn, + LinecastObstacle, + LinecastCost, + ReachedEnd, + } + + /// + /// Simplifies a path on a grid graph using a string pulling algorithm. + /// See the class documentation for more details. + /// + /// A list of input nodes. Only the slice of nodes from nodeStartIndex to nodeEndIndex (inclusive) will be used. These must all be of type GridNodeBase and must form a path (i.e. each node must be a neighbor to the next one in the list). + /// The index in pathNodes to start from. + /// The last index in pathNodes that is used. + /// A more exact start point for the path. This should be a point inside the first node (if not, it will be clamped to the node's surface). + /// A more exact end point for the path. This should be a point inside the first node (if not, it will be clamped to the node's surface). + /// Can be used to specify how much it costs to traverse each node. If this is null, node penalties and tag penalties will be completely ignored. + /// Can be used to filter out additional nodes that should be treated as unwalkable. It is assumed that all nodes in pathNodes pass this filter. + /// If you only need the first N points of the result, you can specify that here, to avoid unnecessary work. + public static List Calculate (List pathNodes, int nodeStartIndex, int nodeEndIndex, Vector3 startPoint, Vector3 endPoint, System.Func traversalCost = null, System.Func filter = null, int maxCorners = int.MaxValue) { + Profiler.BeginSample("Funnel"); + marker6.Begin(); + // A list of indices into the arrays defined below. + // Each index represents a point. But it's more convenient to use indices here and keep all the data separately (also probably faster). + var outputPath = ListPool.Claim(); + outputPath.Add(0); + + var numInputNodes = nodeEndIndex - nodeStartIndex + 1; + var gg = pathNodes[nodeStartIndex].Graph as GridGraph; + var trace = ListPool.Claim(); + var turn = Side.Colinear; + int counter = 0; + + // We will add two points, see comments inside the loop. + // We may end up adding even more points later, therefore we get arrays that are a bit larger than we need for the initial path. + numInputNodes += 2; + int numPoints = numInputNodes; + var nodes = ArrayPool.Claim(numPoints*2); + var points = ArrayPool.Claim(numPoints*2); + var normalizedPoints = ArrayPool.Claim(numPoints*2); + var costs = ArrayPool.Claim(numPoints*2); + + marker7.Begin(); + uint costSoFar = 0; + // Go through all points and store all relevant data we need about them + for (int j = 0; j < numInputNodes; j++) { + // After the start-end modifier has adjusted the endpoints of the path, the line from the start point to the center of the second node in the path + // might not actually have line of sight. + // Assume the path starts at N1 with a diagonal move to node N2. + // The start-end modifier adjusts the start point of the path to point S. + // This causes the path to cut the corner to the unwalkable node in the bottom right. + // ┌─────────┬────────┐ + // │ │ │ + // │ N2 │ │ + // │ \ │ │ + // │ \ │ │ + // ├───────\─┼────────┤ + // │########\│ │ + // │#########│S N1 │ + // │#########│ │ + // │#########│ │ + // └─────────┴────────┘ + // We can solve this case by making the path go from S to N1 and then to N2 instead of directly from S to N2. + // We also do the same thing for the end of the path. + // The clamping and indexing here is essentially equivalent to one insert at the beginning of the arrays and one at the end. + var node = nodes[j] = pathNodes[math.clamp(nodeStartIndex + j-1, nodeStartIndex, nodeEndIndex)] as GridNodeBase; + var gridCoordinates = new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid); + var point = gridCoordinates * FixedPrecisionScale; + int2 normalized; + if (j == 0) { + normalized = ToFixedPrecision(node.NormalizePoint(startPoint)); + normalized = math.clamp(normalized, int2.zero, new int2(FixedPrecisionScale, FixedPrecisionScale)); + } else if (j == numInputNodes - 1) { + normalized = ToFixedPrecision(node.NormalizePoint(endPoint)); + normalized = math.clamp(normalized, int2.zero, new int2(FixedPrecisionScale, FixedPrecisionScale)); + } else { + normalized = new int2(FixedPrecisionScale/2, FixedPrecisionScale/2); + } + points[j] = point + normalized; + normalizedPoints[j] = normalized; + if (j > 0 && traversalCost != null) { + // Calculate the cost of moving along the original path + costSoFar += (uint)(((float)traversalCost(nodes[j-1]) + gg.nodeSize*Int3.Precision) * IntersectionLength(new int2(nodes[j-1].XCoordinateInGrid, nodes[j-1].ZCoordinateInGrid)*FixedPrecisionScale, points[j-1], points[j])); + costSoFar += (uint)(((float)traversalCost(nodes[j]) + gg.nodeSize*Int3.Precision) * IntersectionLength(gridCoordinates*FixedPrecisionScale, points[j-1], points[j])); + } + costs[j] = costSoFar; + } + marker7.End(); + + // We know that there is line of sight from the first point to the second point in the path. + var lastSuccessfulStart = 0; + var lastSuccessfulEnd = 1; + marker6.End(); + + int i = 1; + while (true) { + if (i >= numInputNodes) { + // We are done, add the last point + outputPath.Add(numInputNodes-1); + break; + } + if (outputPath.Count >= maxCorners) { + // We are done with the partial result + break; + } + + counter++; + if (counter > 10000) { + Debug.LogError("Inf loop"); + break; + } + + // In the paper, they just use a straight forward loop over the input path. + // However, it is better for performance to use a binary search to figure out the next time we need to do something. + // We only need an 'i' which succeeds and 'i+1' which fails. + // The success in this case is defined by the predicate below. We only need to do stuff if that returns true. + var last = outputPath[outputPath.Count-1]; + var normalizedLast = normalizedPoints[last]; + var prev = outputPath.Count > 1 ? outputPath[outputPath.Count-2] : -1; + var nodeLast = nodes[last]; + var upperBound = numInputNodes - i - 1; + + // Lower and upper bounds for the binary search + int mn = 0; + // It is reasonable that most paths can be simplified at least a bit. Assume that seeing 4 or more nodes ahead is common. + int mx = math.min(4, upperBound); + var mxFailMode = PredicateFailMode.Undefined; + uint mxLinecastCost = 0; + + // The calculations are not perfectly accurate. Allow the shortcut's cost to be a tiny bit higher. + const uint COST_FUDGE = 5; + + GridHitInfo hit; + // First fire off linecasts to nodes exponentially further away until the predicate returns true. + while (true) { + var idx = i + mx; + + var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[idx]) != turn; + if (turnPredicate) { + mxFailMode = PredicateFailMode.Turn; + break; + } else { + trace.Clear(); + if (gg.Linecast(nodeLast, normalizedLast, nodes[idx], normalizedPoints[idx], out hit, trace, filter)) { + mxFailMode = PredicateFailMode.LinecastObstacle; + break; + } else if (traversalCost != null) { + var cost = LinecastCost(trace, points[last], points[idx], gg, traversalCost); + if (cost > costs[idx] - costs[last] + COST_FUDGE) { + // The "shortcut" had such a high penalty that it's not worth taking it + mxFailMode = PredicateFailMode.LinecastCost; + mxLinecastCost = cost; + break; + } + } + } + + if (mx < upperBound) { + mn = mx; + mx = math.min(mx*2, upperBound); + } else { + mxFailMode = PredicateFailMode.ReachedEnd; + break; + } + } + + if (mxFailMode == PredicateFailMode.ReachedEnd) { + // Reached final node without any hits, we can stop here + outputPath.Add(numInputNodes-1); + break; + } + + // Run a standard binary search + while (mx > mn + 1) { + int mid = (mn+mx)/2; + int idx = i + mid; + + var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[idx]) != turn; + bool pred = turnPredicate; + if (turnPredicate) { + mxFailMode = PredicateFailMode.Turn; + } else { + trace.Clear(); + if (gg.Linecast(nodeLast, normalizedLast, nodes[idx], normalizedPoints[idx], out hit, trace, filter)) { + mxFailMode = PredicateFailMode.LinecastObstacle; + pred = true; + } else if (traversalCost != null) { + var cost = LinecastCost(trace, points[last], points[idx], gg, traversalCost); + if (cost > costs[idx] - costs[last] + COST_FUDGE) { + // The "shortcut" had such a high penalty that it's not worth taking it + mxFailMode = PredicateFailMode.LinecastCost; + mxLinecastCost = cost; + pred = true; + } + } + } + + if (pred) { + mx = mid; + } else { + mn = mid; + } + } + + // i+mn is now a succeeding index, and i+mn+1 (or i+mx) is a failing index + if (mn > 0) { + lastSuccessfulStart = last; + lastSuccessfulEnd = i+mn; + } else { + // We are not actually completely sure that i+mn is a succeeding index if mn=0 + // So double check it. + // TODO: This is a lot of code duplication. Tidy this up. + var turnPredicate = outputPath.Count > 1 && Side2D(points[prev], points[last], points[i+mn]) != turn; + bool pred = turnPredicate; + if (turnPredicate) { + } else { + trace.Clear(); + if (gg.Linecast(nodeLast, normalizedLast, nodes[i+mn], normalizedPoints[i+mn], out hit, trace, filter)) { + pred = true; + } else if (traversalCost != null) { + var cost = LinecastCost(trace, points[last], points[i+mn], gg, traversalCost); + if (cost > costs[i+mn] - costs[last] + COST_FUDGE) { + // The "shortcut" had such a high penalty that it's not worth taking it + mxLinecastCost = cost; + pred = true; + } + } + } + + if (!pred) { + // Success! + lastSuccessfulStart = last; + lastSuccessfulEnd = i+mn; + } + } + + // Move to the failing index + i += mx; + UnityEngine.Assertions.Assert.AreNotEqual(mxFailMode, PredicateFailMode.Undefined); + + marker5.Begin(); + trace.Clear(); + trace.Clear(); + if (mxFailMode == PredicateFailMode.LinecastCost) { + outputPath.Add(lastSuccessfulEnd); + turn = Side2D(points[last], points[lastSuccessfulEnd], points[i]); + // It is guaranteed that there is line of sight from lastSuccessfulStart to lastSuccessfulEnd + lastSuccessfulStart = lastSuccessfulEnd; + i--; + marker5.End(); + continue; + } else if (mxFailMode == PredicateFailMode.LinecastObstacle) { + marker5.End(); + // Draw.Line(nodes[last].UnNormalizePoint(FromFixedPrecision(normalizedPoints[last])), toNode.UnNormalizePoint(FromFixedPrecision(normalizedTo)), Color.red); + marker1.Begin(); + marker3.Begin(); + // Re-run a previously successfully linecast to get all nodes it traversed. + trace.Clear(); + int chosenCorner; + if (gg.Linecast(nodes[lastSuccessfulStart], normalizedPoints[lastSuccessfulStart], nodes[lastSuccessfulEnd], normalizedPoints[lastSuccessfulEnd], out hit, trace, filter)) { + // Weird! This linecast should have succeeded. + // Maybe the path crosses some unwalkable nodes it shouldn't cross (the graph could have changed). + // Or possibly the linecast implementation doesn't handle some edge case (there are so many!) + // In any case, we fall back to just assuming there is a valid line of sight. + chosenCorner = lastSuccessfulEnd; + Debug.LogError("Inconsistent linecasts"); + } else { + trace.Add(nodes[i]); + marker3.End(); + marker4.Begin(); + + GridNodeBase candidateNode = null; + var candidateNormalizedPoint = new int2(); + uint candidateCost = 0; + var dirToCandidateCorner = new int2(); + var lastSuccessfulStartPoint = points[lastSuccessfulStart]; + var lastSuccessfulEndPoint = points[lastSuccessfulEnd]; + var dir = lastSuccessfulEndPoint - lastSuccessfulStartPoint; + var bounds = new TriangleBounds( + lastSuccessfulStartPoint, + lastSuccessfulEndPoint, + points[i] + ); + + var desiredSide = System.Math.Sign(Cross(dir, points[i] - lastSuccessfulStartPoint)); + var candidateCostSoFar = costs[lastSuccessfulStart]; + for (int j = 0; j < trace.Count; j++) { + var node = trace[j] as GridNodeBase; + var nodeGridPos = new int2(node.XCoordinateInGrid, node.ZCoordinateInGrid); + var nodeCenter = nodeGridPos * FixedPrecisionScale; + if (traversalCost != null) { + // Not perfectly accurate as it doesn't measure the cost to the exact corner + candidateCostSoFar += (uint)(((float)traversalCost(node) + gg.nodeSize*Int3.Precision) * IntersectionLength(nodeCenter, lastSuccessfulStartPoint, lastSuccessfulEndPoint)); + } + for (int d = 0; d < 4; d++) { + if (!node.HasConnectionInDirection(d) || (filter != null && !filter(node.GetNeighbourAlongDirection(d)))) { + for (int q = 0; q < 2; q++) { + var ncorner = directionToCorners[(d+q)&0x3]; + var corner = nodeCenter + ncorner; + + if (!bounds.Contains(corner)) { + continue; + } + + var dirToCorner = corner - lastSuccessfulStartPoint; + // We shouldn't pick corners at our current position + if (math.all(dirToCorner == 0)) continue; + if (math.all(corner == lastSuccessfulEndPoint)) continue; + + var side = Cross(dirToCorner, dirToCandidateCorner); + if (candidateNode == null || System.Math.Sign(side) == desiredSide || (side == 0 && math.lengthsq(dirToCorner) > math.lengthsq(dirToCandidateCorner))) { + dirToCandidateCorner = dirToCorner; + candidateNode = node; + candidateNormalizedPoint = ncorner; + candidateCost = candidateCostSoFar; + } + } + } + } + } + marker4.End(); + + if (candidateNode == null) { + // Fall back to adding the lastSuccessfulEnd node. We know there's line of sight to that one. + chosenCorner = lastSuccessfulEnd; + } else { + chosenCorner = numPoints; + // TODO: Reallocate + nodes[numPoints] = candidateNode; + normalizedPoints[numPoints] = candidateNormalizedPoint; + var gridCoordinates = new int2(candidateNode.XCoordinateInGrid, candidateNode.ZCoordinateInGrid); + points[numPoints] = gridCoordinates * FixedPrecisionScale + candidateNormalizedPoint; + costs[numPoints] = candidateCost; + numPoints++; + } + } + + outputPath.Add(chosenCorner); + turn = Side2D(points[last], points[chosenCorner], points[i]); + // It is guaranteed that there is line of sight from lastSuccessfulStart to chosenCorner because of how we choose the corner. + lastSuccessfulStart = chosenCorner; + i--; + marker1.End(); + continue; + } else { + marker5.End(); + marker2.Begin(); + lastSuccessfulStart = last; + lastSuccessfulEnd = i; + // Draw.Line(nodes[last].UnNormalizePoint(FromFixedPrecision(normalizedPoints[last])), toNode.UnNormalizePoint(FromFixedPrecision(normalizedTo)), Color.green); + if (outputPath.Count > 1) { + var spPrev = outputPath[outputPath.Count-2]; + var nextTurn = Side2D(points[spPrev], points[last], points[i]); + // Check if the string is no longer taut. If it is not we can remove a previous point. + if (turn != nextTurn) { + // Draw.SphereOutline(nodes[pts[pts.Count-1]].UnNormalizePoint(FromFixedPrecision(normalizedPoints[pts[pts.Count-1]])), 0.05f, Color.black); + + lastSuccessfulStart = outputPath[outputPath.Count-2]; + lastSuccessfulEnd = outputPath[outputPath.Count-1]; + + outputPath.RemoveAt(outputPath.Count-1); + if (outputPath.Count > 1) { + last = spPrev; + spPrev = outputPath[outputPath.Count-2]; + turn = Side2D(points[spPrev], points[last], points[i]); + } else { + // TODO: Should be separate value + turn = Side.Colinear; + } + i--; + marker2.End(); + continue; + } + } + marker2.End(); + } + } + + Profiler.EndSample(); + + var result = ListPool.Claim(outputPath.Count); + for (int j = 0; j < outputPath.Count; j++) { + var idx = outputPath[j]; + result.Add(nodes[idx].UnNormalizePoint(FromFixedPrecision(normalizedPoints[idx]))); + } + + ArrayPool.Release(ref nodes); + ArrayPool.Release(ref points); + ArrayPool.Release(ref normalizedPoints); + ArrayPool.Release(ref costs); + ListPool.Release(ref outputPath); + ListPool.Release(ref trace); + return result; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs.meta new file mode 100644 index 0000000..dc13a89 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GridStringPulling.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 138b968807644d3ac8c22ad9d087f012 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/IJobParallelForBatched.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/IJobParallelForBatched.cs new file mode 100644 index 0000000..761993b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/IJobParallelForBatched.cs @@ -0,0 +1,65 @@ +// This file is only included because the Unity.Jobs package is currently experimental and it seems bad to rely on it. +// The Unity.Jobs version of this interface will be used when it is stable. +using System; +using Unity.Jobs.LowLevel.Unsafe; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; + +namespace Pathfinding.Jobs { + [JobProducerType(typeof(JobParallelForBatchedExtensions.ParallelForBatchJobStruct<>))] + public interface IJobParallelForBatched { + bool allowBoundsChecks { get; } + void Execute(int startIndex, int count); + } + + public static class JobParallelForBatchedExtensions { + internal struct ParallelForBatchJobStruct where T : struct, IJobParallelForBatched { + static public IntPtr jobReflectionData; + + public static IntPtr Initialize () { + if (jobReflectionData == IntPtr.Zero) { +#if UNITY_2020_2_OR_NEWER + jobReflectionData = JobsUtility.CreateJobReflectionData(typeof(T), (ExecuteJobFunction)Execute, null, null); +#else + jobReflectionData = JobsUtility.CreateJobReflectionData(typeof(T), JobType.ParallelFor, (ExecuteJobFunction)Execute); +#endif + } + return jobReflectionData; + } + + public delegate void ExecuteJobFunction(ref T data, System.IntPtr additionalPtr, System.IntPtr bufferRangePatchData, ref JobRanges ranges, int jobIndex); + public unsafe static void Execute (ref T jobData, System.IntPtr additionalPtr, System.IntPtr bufferRangePatchData, ref JobRanges ranges, int jobIndex) { + while (true) { + int begin; + int end; + if (!JobsUtility.GetWorkStealingRange(ref ranges, jobIndex, out begin, out end)) + return; + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (jobData.allowBoundsChecks) JobsUtility.PatchBufferMinMaxRanges(bufferRangePatchData, UnsafeUtility.AddressOf(ref jobData), begin, end - begin); +#endif + + jobData.Execute(begin, end - begin); + } + } + } + + unsafe static public JobHandle ScheduleBatch(this T jobData, int arrayLength, int minIndicesPerJobCount, JobHandle dependsOn = new JobHandle()) where T : struct, IJobParallelForBatched { +#if UNITY_2020_2_OR_NEWER + // This was renamed in Unity 2020.2 + var scheduleMode = ScheduleMode.Parallel; +#else + var scheduleMode = ScheduleMode.Batched; +#endif + var scheduleParams = new JobsUtility.JobScheduleParameters(UnsafeUtility.AddressOf(ref jobData), ParallelForBatchJobStruct.Initialize(), dependsOn, scheduleMode); + + return JobsUtility.ScheduleParallelFor(ref scheduleParams, arrayLength, minIndicesPerJobCount); + } + + unsafe static public void RunBatch(this T jobData, int arrayLength) where T : struct, IJobParallelForBatched { + var scheduleParams = new JobsUtility.JobScheduleParameters(UnsafeUtility.AddressOf(ref jobData), ParallelForBatchJobStruct.Initialize(), new JobHandle(), ScheduleMode.Run); + + JobsUtility.ScheduleParallelFor(ref scheduleParams, arrayLength, arrayLength); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/IJobParallelForBatched.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/IJobParallelForBatched.cs.meta new file mode 100644 index 0000000..73941ef --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/IJobParallelForBatched.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a27178adf123341a3b302184fe478f8b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/JobDependencyTracker.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/JobDependencyTracker.cs new file mode 100644 index 0000000..19bb3b9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/JobDependencyTracker.cs @@ -0,0 +1,799 @@ +// #define DEBUG_JOBS +namespace Pathfinding.Jobs { + using System.Reflection; + using Unity.Collections; + using Unity.Jobs; + using System.Collections.Generic; + using Unity.Collections.LowLevel.Unsafe; + using Pathfinding.Util; + using System.Runtime.InteropServices; + using System.Diagnostics; + + /// + /// Disable the check that prevents jobs from including uninitialized native arrays open for reading. + /// + /// Sometimes jobs have to include a readable native array that starts out uninitialized. + /// The job might for example write to it and later read from it in the same job. + /// + /// See: + /// + class DisableUninitializedReadCheckAttribute : System.Attribute { + } + + public interface IArenaDisposable { + void DisposeWith(DisposeArena arena); + } + + /// Convenient collection of items that can be disposed together + public class DisposeArena { + List > buffer; + List > buffer2; + List > buffer3; + List gcHandles; + + public void Add(NativeArray data) where T : unmanaged { + if (buffer == null) buffer = ListPool >.Claim(); + buffer.Add(data.Reinterpret(UnsafeUtility.SizeOf())); + } + + public void Add(NativeList data) where T : unmanaged { + // SAFETY: This is safe because NativeList and NativeList have the same memory layout. + var byteList = Unity.Collections.LowLevel.Unsafe.UnsafeUtility.As, NativeList >(ref data); + if (buffer2 == null) buffer2 = ListPool >.Claim(); + buffer2.Add(byteList); + } + + public void Add(NativeQueue data) where T : unmanaged { + // SAFETY: This is safe because NativeQueue and NativeQueue have the same memory layout. + var byteList = Unity.Collections.LowLevel.Unsafe.UnsafeUtility.As, NativeQueue >(ref data); + if (buffer3 == null) buffer3 = ListPool >.Claim(); + buffer3.Add(byteList); + } + + public void Remove(NativeArray data) where T : unmanaged { + if (buffer == null) return; + unsafe { + var ptr = NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(data); + for (int i = 0; i < buffer.Count; i++) { + if (NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(buffer[i]) == ptr) { + buffer.RemoveAtSwapBack(i); + return; + } + } + } + } + + public void Add(T data) where T : IArenaDisposable { + data.DisposeWith(this); + } + + public void Add (GCHandle handle) { + if (gcHandles == null) gcHandles = ListPool.Claim(); + gcHandles.Add(handle); + } + + /// + /// Dispose all items in the arena. + /// This also clears the arena and makes it available for reuse. + /// + public void DisposeAll () { + UnityEngine.Profiling.Profiler.BeginSample("Disposing"); + if (buffer != null) { + for (int i = 0; i < buffer.Count; i++) buffer[i].Dispose(); + ListPool >.Release(ref buffer); + } + if (buffer2 != null) { + for (int i = 0; i < buffer2.Count; i++) buffer2[i].Dispose(); + ListPool >.Release(ref buffer2); + } + if (buffer3 != null) { + for (int i = 0; i < buffer3.Count; i++) buffer3[i].Dispose(); + ListPool >.Release(ref buffer3); + } + if (gcHandles != null) { + for (int i = 0; i < gcHandles.Count; i++) gcHandles[i].Free(); + ListPool.Release(ref gcHandles); + } + UnityEngine.Profiling.Profiler.EndSample(); + } + } + + // TODO: Remove or use? + public struct JobHandleWithMainThreadWork where T : struct { + JobDependencyTracker tracker; + IEnumerator<(JobHandle, T)> coroutine; + + public JobHandleWithMainThreadWork (IEnumerator<(JobHandle, T)> handles, JobDependencyTracker tracker) { + this.coroutine = handles; + this.tracker = tracker; + } + + public void Complete () { + tracker.timeSlice = TimeSlice.Infinite; + while (coroutine.MoveNext()) { + coroutine.Current.Item1.Complete(); + } + } + + public System.Collections.Generic.IEnumerable CompleteTimeSliced (float maxMillisPerStep) { + tracker.timeSlice = TimeSlice.MillisFromNow(maxMillisPerStep); + while (true) { + if (!coroutine.MoveNext()) yield break; + if (maxMillisPerStep < float.PositiveInfinity) { + while (!coroutine.Current.Item1.IsCompleted) { + yield return null; + tracker.timeSlice = TimeSlice.MillisFromNow(maxMillisPerStep); + } + } + coroutine.Current.Item1.Complete(); + yield return coroutine.Current.Item2; + tracker.timeSlice = TimeSlice.MillisFromNow(maxMillisPerStep); + } + } + } + + enum LinearDependencies : byte { + Check, + Enabled, + Disabled, + } + + /// + /// Automatic dependency tracking for the Unity Job System. + /// + /// Uses reflection to find the [ReadOnly] and [WriteOnly] attributes on job data struct fields. + /// These are used to automatically figure out dependencies between jobs. + /// + /// A job that reads from an array depends on the last job that wrote to that array. + /// A job that writes to an array depends on the last job that wrote to the array as well as all jobs that read from the array. + /// + /// + /// struct ExampleJob : IJob { + /// public NativeArray someData; + /// + /// public void Execute () { + /// // Do something + /// } + /// } + /// + /// void Start () { + /// var tracker = new JobDependencyTracker(); + /// var data = new NativeArray(100, Allocator.TempJob); + /// var job1 = new ExampleJob { + /// someData = data + /// }.Schedule(tracker); + /// + /// var job2 = new ExampleJob { + /// someData = data + /// }.Schedule(tracker); + /// + /// // job2 automatically depends on job1 because they both require read/write access to the data array + /// } + /// + /// + /// See: + /// + public class JobDependencyTracker : IAstarPooledObject { + internal List slots = ListPool.Claim(); + DisposeArena arena; + internal NativeArray dependenciesScratchBuffer; + LinearDependencies linearDependencies; + internal TimeSlice timeSlice = TimeSlice.Infinite; + + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + ~JobDependencyTracker() { + if (dependenciesScratchBuffer.IsCreated) { + UnityEngine.Debug.LogError("JobDependencyTracker was not disposed. This will cause a memory leak. Please call Dispose on the JobDependencyTracker when you are done with it."); + } + } +#endif + + public bool forceLinearDependencies { + get { + if (linearDependencies == LinearDependencies.Check) SetLinearDependencies(false); + return linearDependencies == LinearDependencies.Enabled; + } + } + + internal struct JobInstance { + public JobHandle handle; + public int hash; +#if DEBUG_JOBS + public string name; +#endif + } + + internal struct NativeArraySlot { + public long hash; + public JobInstance lastWrite; + public List lastReads; + public bool initialized; + public bool hasWrite; + } + + // Note: burst compiling even an empty job can avoid the overhead of going from unmanaged to managed code. + /* [BurstCompile] + struct JobDispose : IJob where T : struct { + [DeallocateOnJobCompletion] + [DisableUninitializedReadCheck] + public NativeArray data; + + public void Execute () { + } + }*/ + + struct JobRaycastCommandDummy : IJob { + [ReadOnly] + public NativeArray commands; + [WriteOnly] + public NativeArray results; + + public void Execute () {} + } + +#if UNITY_2022_2_OR_NEWER + struct JobOverlapCapsuleCommandDummy : IJob { + [ReadOnly] + public NativeArray commands; + [WriteOnly] + public NativeArray results; + + public void Execute () {} + } + + struct JobOverlapSphereCommandDummy : IJob { + [ReadOnly] + public NativeArray commands; + [WriteOnly] + public NativeArray results; + + public void Execute () {} + } +#endif + + /// + /// JobHandle that represents a dependency for all jobs. + /// All native arrays that are written (and have been tracked by this tracker) to will have their final results in them + /// when the returned job handle is complete. + /// + public JobHandle AllWritesDependency { + get { + var handles = new NativeArray(slots.Count, Allocator.Temp); + for (int i = 0; i < slots.Count; i++) handles[i] = slots[i].lastWrite.handle; + var dependencies = JobHandle.CombineDependencies(handles); + handles.Dispose(); + return dependencies; + } + } + + bool supportsMultithreading { + get { +#if UNITY_WEBGL + return false; +#else + return Unity.Jobs.LowLevel.Unsafe.JobsUtility.JobWorkerCount > 0; +#endif + } + } + + /// + /// Disable dependency tracking and just run jobs one after the other. + /// This may be faster in some cases since dependency tracking has some overhead. + /// + public void SetLinearDependencies (bool linearDependencies) { + if (!supportsMultithreading) linearDependencies = true; + + if (linearDependencies) { + AllWritesDependency.Complete(); + } + this.linearDependencies = linearDependencies ? LinearDependencies.Enabled : LinearDependencies.Disabled; + } + + public NativeArray NewNativeArray(int length, Allocator allocator, NativeArrayOptions options = NativeArrayOptions.ClearMemory) where T : unmanaged { + var res = new NativeArray(length, allocator, options); + Track(res, options == NativeArrayOptions.ClearMemory); + return res; + } + + public void Track(NativeArray array, bool initialized = true) where T : unmanaged { + unsafe { + slots.Add(new NativeArraySlot { + hash = (long)NativeArrayUnsafeUtility.GetUnsafeBufferPointerWithoutChecks(array), + lastWrite = default, + lastReads = ListPool.Claim(), + initialized = initialized, + }); + } + if (this.arena == null) this.arena = new DisposeArena(); + arena.Add(array); + } + + /// + /// Makes the given array not be disposed when this tracker is disposed. + /// This is useful if you want to keep the array around after the tracker has been disposed. + /// The array will still be tracked for the purposes of automatic dependency management. + /// + public void Persist(NativeArray array) where T : unmanaged { + if (this.arena == null) return; + arena.Remove(array); + } + + /// + /// Schedules a raycast batch command. + /// Like RaycastCommand.ScheduleBatch, but dependencies are tracked automatically. + /// + public JobHandle ScheduleBatch (NativeArray commands, NativeArray results, int minCommandsPerJob) { + if (forceLinearDependencies) { + UnityEngine.RaycastCommand.ScheduleBatch(commands, results, minCommandsPerJob).Complete(); + return default; + } + + // Create a dummy structure to allow the analyzer to determine how the job reads/writes data + var dummy = new JobRaycastCommandDummy { commands = commands, results = results }; + var dependencies = JobDependencyAnalyzer.GetDependencies(ref dummy, this); + var job = UnityEngine.RaycastCommand.ScheduleBatch(commands, results, minCommandsPerJob, dependencies); + + JobDependencyAnalyzer.Scheduled(ref dummy, this, job); + return job; + } + +#if UNITY_2022_2_OR_NEWER + /// + /// Schedules an overlap capsule batch command. + /// Like OverlapCapsuleCommand.ScheduleBatch, but dependencies are tracked automatically. + /// + public JobHandle ScheduleBatch (NativeArray commands, NativeArray results, int minCommandsPerJob) { + if (forceLinearDependencies) { + UnityEngine.OverlapCapsuleCommand.ScheduleBatch(commands, results, minCommandsPerJob, 1).Complete(); + return default; + } + + // Create a dummy structure to allow the analyzer to determine how the job reads/writes data + var dummy = new JobOverlapCapsuleCommandDummy { commands = commands, results = results }; + var dependencies = JobDependencyAnalyzer.GetDependencies(ref dummy, this); + var job = UnityEngine.OverlapCapsuleCommand.ScheduleBatch(commands, results, minCommandsPerJob, 1, dependencies); + + JobDependencyAnalyzer.Scheduled(ref dummy, this, job); + return job; + } + + /// + /// Schedules an overlap sphere batch command. + /// Like OverlapSphereCommand.ScheduleBatch, but dependencies are tracked automatically. + /// + public JobHandle ScheduleBatch (NativeArray commands, NativeArray results, int minCommandsPerJob) { + if (forceLinearDependencies) { + UnityEngine.OverlapSphereCommand.ScheduleBatch(commands, results, minCommandsPerJob, 1).Complete(); + return default; + } + + // Create a dummy structure to allow the analyzer to determine how the job reads/writes data + var dummy = new JobOverlapSphereCommandDummy { commands = commands, results = results }; + var dependencies = JobDependencyAnalyzer.GetDependencies(ref dummy, this); + var job = UnityEngine.OverlapSphereCommand.ScheduleBatch(commands, results, minCommandsPerJob, 1, dependencies); + + JobDependencyAnalyzer.Scheduled(ref dummy, this, job); + return job; + } +#endif + + /// Frees the GCHandle when the JobDependencyTracker is disposed + public void DeferFree (GCHandle handle, JobHandle dependsOn) { + if (this.arena == null) this.arena = new DisposeArena(); + this.arena.Add(handle); + } + +#if DEBUG_JOBS + internal void JobReadsFrom (JobHandle job, long nativeArrayHash, int jobHash, string jobName) +#else + internal void JobReadsFrom (JobHandle job, long nativeArrayHash, int jobHash) +#endif + { + for (int j = 0; j < slots.Count; j++) { + var slot = slots[j]; + if (slot.hash == nativeArrayHash) { + // If the job only reads from the array then we just add this job to the list of readers + slot.lastReads.Add(new JobInstance { + handle = job, + hash = jobHash, +#if DEBUG_JOBS + name = jobName, +#endif + }); + break; + } + } + } + +#if DEBUG_JOBS + internal void JobWritesTo (JobHandle job, long nativeArrayHash, int jobHash, string jobName) +#else + internal void JobWritesTo (JobHandle job, long nativeArrayHash, int jobHash) +#endif + { + for (int j = 0; j < slots.Count; j++) { + var slot = slots[j]; + if (slot.hash == nativeArrayHash) { + // If the job writes to the array then this job is now the last writer + slot.lastWrite = new JobInstance { + handle = job, + hash = jobHash, +#if DEBUG_JOBS + name = jobName, +#endif + }; + slot.lastReads.Clear(); + // The array no longer contains uninitialized data. + // Parts of it may still be uninitialized if the job doesn't write to everything, but that's something that this class cannot track. + slot.initialized = true; + slot.hasWrite = true; + slots[j] = slot; + break; + } + } + } + + /// + /// Disposes this tracker. + /// This will pool all used lists which makes the GC happy. + /// + /// Note: It is necessary to call this method to avoid memory leaks if you are using the DeferDispose method. But it's a good thing to do otherwise as well. + /// It is automatically called if you are using the ObjectPool.Release method. + /// + void Dispose () { +#if ENABLE_UNITY_COLLECTIONS_CHECKS && UNITY_2022_2_OR_NEWER + // Note: This can somehow fail in Unity 2021 and 2022.1, even when calling Complete on all jobs + UnityEngine.Assertions.Assert.IsTrue(AllWritesDependency.IsCompleted); +#endif + for (int i = 0; i < slots.Count; i++) ListPool.Release(slots[i].lastReads); + + slots.Clear(); + if (arena != null) arena.DisposeAll(); + linearDependencies = LinearDependencies.Check; + if (dependenciesScratchBuffer.IsCreated) dependenciesScratchBuffer.Dispose(); + } + + public void ClearMemory () { + AllWritesDependency.Complete(); + Dispose(); + } + + void IAstarPooledObject.OnEnterPool () { + Dispose(); + } + } + + public struct TimeSlice { + public long endTick; + public static readonly TimeSlice Infinite = new TimeSlice { endTick = long.MaxValue }; + public bool expired => Stopwatch.GetTimestamp() > endTick; + + public static TimeSlice MillisFromNow (float millis) => new TimeSlice { endTick = Stopwatch.GetTimestamp() + (long)(millis * 10000) }; + } + + public interface IJobTimeSliced : IJob { + /// + /// Returns true if the job completed. + /// If false is returned this job may be called again until the job completes. + /// + bool Execute(TimeSlice timeSlice); + } + + /// Extension methods for IJob and related interfaces + public static class IJobExtensions { + struct ManagedJob : IJob { + public GCHandle handle; + + public void Execute () { + ((IJob)handle.Target).Execute(); + handle.Free(); + } + } + + struct ManagedActionJob : IJob { + public GCHandle handle; + + public void Execute () { + ((System.Action)handle.Target)(); + handle.Free(); + } + } + + /// + /// Schedule a job with automatic dependency tracking. + /// You need to have "using Pathfinding.Util" in your script to be able to use this extension method. + /// + /// See: + /// + // TODO: Compare performance impact by using ref this, and ScheduleByRef + public static JobHandle Schedule(this T data, JobDependencyTracker tracker) where T : struct, IJob { + if (tracker.forceLinearDependencies) { + data.Run(); + return default; + } else { + var job = data.Schedule(JobDependencyAnalyzer.GetDependencies(ref data, tracker)); + JobDependencyAnalyzer.Scheduled(ref data, tracker, job); + return job; + } + } + + /// Schedules an job with automatic dependency tracking + public static JobHandle ScheduleBatch(this T data, int arrayLength, int minIndicesPerJobCount, JobDependencyTracker tracker, JobHandle additionalDependency = default) where T : struct, IJobParallelForBatched { + if (tracker.forceLinearDependencies) { + additionalDependency.Complete(); + //data.ScheduleBatch(arrayLength, minIndicesPerJobCount, additionalDependency).Complete(); + data.RunBatch(arrayLength); + return default; + } else { + var job = data.ScheduleBatch(arrayLength, minIndicesPerJobCount, JobDependencyAnalyzer.GetDependencies(ref data, tracker, additionalDependency)); + + JobDependencyAnalyzer.Scheduled(ref data, tracker, job); + return job; + } + } + + /// Schedules a managed job to run in the job system + public static JobHandle ScheduleManaged(this T data, JobHandle dependsOn) where T : struct, IJob { + return new ManagedJob { handle = GCHandle.Alloc(data) }.Schedule(dependsOn); + } + + /// Schedules a managed job to run in the job system + public static JobHandle ScheduleManaged (this System.Action data, JobHandle dependsOn) { + return new ManagedActionJob { + handle = GCHandle.Alloc(data) + }.Schedule(dependsOn); + } + + public static JobHandle GetDependencies(this T data, JobDependencyTracker tracker) where T : struct, IJob { + if (tracker.forceLinearDependencies) return default; + else return JobDependencyAnalyzer.GetDependencies(ref data, tracker); + } + + /// + /// Executes this job in the main thread using a coroutine. + /// Usage: + /// - 1. Optionally schedule some other jobs before this one (using the dependency tracker) + /// - 2. Call job.ExecuteMainThreadJob(tracker) + /// - 3. Iterate over the enumerator until it is finished. Call handle.Complete on all yielded job handles. Usually this only yields once, but if you use the wrapper it will + /// yield once for every time slice. + /// - 4. Continue scheduling other jobs. + /// + /// You must not schedule other jobs (that may touch the same data) while executing this job. + /// + /// See: + /// + public static IEnumerator ExecuteMainThreadJob(this T data, JobDependencyTracker tracker) where T : struct, IJobTimeSliced { + if (tracker.forceLinearDependencies) { + UnityEngine.Profiling.Profiler.BeginSample("Main Thread Work"); + data.Execute(); + UnityEngine.Profiling.Profiler.EndSample(); + yield break; + } + + var dependsOn = JobDependencyAnalyzer.GetDependencies(ref data, tracker); + yield return dependsOn; + + while (true) { + UnityEngine.Profiling.Profiler.BeginSample("Main Thread Work"); + var didComplete = data.Execute(tracker.timeSlice); + UnityEngine.Profiling.Profiler.EndSample(); + if (didComplete) yield break; + else yield return new JobHandle(); + } + } + } + + internal static class JobDependencyAnalyzerAssociated { + internal static UnityEngine.Profiling.CustomSampler getDependenciesSampler = UnityEngine.Profiling.CustomSampler.Create("GetDependencies"); + internal static UnityEngine.Profiling.CustomSampler iteratingSlotsSampler = UnityEngine.Profiling.CustomSampler.Create("IteratingSlots"); + internal static UnityEngine.Profiling.CustomSampler initSampler = UnityEngine.Profiling.CustomSampler.Create("Init"); + internal static UnityEngine.Profiling.CustomSampler combineSampler = UnityEngine.Profiling.CustomSampler.Create("Combining"); + internal static int[] tempJobDependencyHashes = new int[16]; + internal static int jobCounter = 1; + } + + struct JobDependencyAnalyzer where T : struct { + static ReflectionData reflectionData; + + /// Offset to the m_Buffer field inside each NativeArray + // Note: Due to a Unity bug we have to calculate this for NativeArray instead of NativeArray<>. NativeArray<> will return an incorrect value (-16) when using IL2CPP. + static readonly int BufferOffset = UnsafeUtility.GetFieldOffset(typeof(NativeArray).GetField("m_Buffer", BindingFlags.Instance | BindingFlags.NonPublic)); + static readonly int SpanPtrOffset = UnsafeUtility.GetFieldOffset(typeof(UnsafeSpan).GetField("ptr", BindingFlags.Instance | BindingFlags.NonPublic)); + struct ReflectionData { + public int[] fieldOffsets; + public bool[] writes; + public bool[] checkUninitializedRead; + public string[] fieldNames; + + public void Build () { + // Find the byte offsets within the struct to all m_Buffer fields in all the native arrays in the struct + var fields = new List(); + var writes = new List(); + var reads = new List(); + var names = new List(); + + Build(typeof(T), fields, writes, reads, names, 0, false, false, false); + this.fieldOffsets = fields.ToArray(); + this.writes = writes.ToArray(); + this.fieldNames = names.ToArray(); + this.checkUninitializedRead = reads.ToArray(); + } + + void Build (System.Type type, List fields, List writes, List reads, List names, int offset, bool forceReadOnly, bool forceWriteOnly, bool forceDisableUninitializedCheck) { + foreach (var field in type.GetFields(BindingFlags.Instance | BindingFlags.Public | BindingFlags.NonPublic)) { + if (field.FieldType.IsGenericType && field.FieldType.GetGenericTypeDefinition() == typeof(NativeArray<>)) { + // Handle NativeArrays + fields.Add(offset + UnsafeUtility.GetFieldOffset(field) + BufferOffset); + writes.Add(!forceReadOnly && field.GetCustomAttribute(typeof(ReadOnlyAttribute)) == null); + reads.Add(!forceWriteOnly && !forceDisableUninitializedCheck && field.GetCustomAttribute(typeof(WriteOnlyAttribute)) == null && field.GetCustomAttribute(typeof(DisableUninitializedReadCheckAttribute)) == null); + names.Add(field.Name); + } else if (field.FieldType.IsGenericType && field.FieldType.GetGenericTypeDefinition() == typeof(UnsafeSpan<>)) { + // Handle UnsafeSpans + fields.Add(offset + UnsafeUtility.GetFieldOffset(field) + SpanPtrOffset); + writes.Add(!forceReadOnly && field.GetCustomAttribute(typeof(ReadOnlyAttribute)) == null); + reads.Add(!forceWriteOnly && !forceDisableUninitializedCheck && field.GetCustomAttribute(typeof(WriteOnlyAttribute)) == null && field.GetCustomAttribute(typeof(DisableUninitializedReadCheckAttribute)) == null); + names.Add(field.Name); + } else if (!field.FieldType.IsPrimitive && field.FieldType.IsValueType && !field.FieldType.IsEnum) { + // Recurse to handle nested types + bool readOnly = field.GetCustomAttribute(typeof(ReadOnlyAttribute)) != null; + bool writeOnly = field.GetCustomAttribute(typeof(WriteOnlyAttribute)) != null; + bool disableUninitializedCheck = field.GetCustomAttribute(typeof(DisableUninitializedReadCheckAttribute)) != null; + Build(field.FieldType, fields, writes, reads, names, offset + UnsafeUtility.GetFieldOffset(field), readOnly, writeOnly, disableUninitializedCheck); + } + } + } + } + + static void initReflectionData () { + if (reflectionData.fieldOffsets == null) { + reflectionData.Build(); + } + } + + static bool HasHash (int[] hashes, int hash, int count) { + for (int i = 0; i < count; i++) if (hashes[i] == hash) return true; + return false; + } + + /// Returns the dependencies for the given job. + /// Job data. Must be allocated on the stack. + /// The tracker to use for dependency tracking. + public static JobHandle GetDependencies (ref T data, JobDependencyTracker tracker) { + return GetDependencies(ref data, tracker, default, false); + } + + public static JobHandle GetDependencies (ref T data, JobDependencyTracker tracker, JobHandle additionalDependency) { + return GetDependencies(ref data, tracker, additionalDependency, true); + } + + static JobHandle GetDependencies (ref T data, JobDependencyTracker tracker, JobHandle additionalDependency, bool useAdditionalDependency) { + //JobDependencyAnalyzerAssociated.getDependenciesSampler.Begin(); + //JobDependencyAnalyzerAssociated.initSampler.Begin(); + if (!tracker.dependenciesScratchBuffer.IsCreated) tracker.dependenciesScratchBuffer = new NativeArray(16, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + var dependencies = tracker.dependenciesScratchBuffer; + var slots = tracker.slots; + var dependencyHashes = JobDependencyAnalyzerAssociated.tempJobDependencyHashes; + + int numDependencies = 0; + + //JobDependencyAnalyzerAssociated.initSampler.End(); + initReflectionData(); +#if DEBUG_JOBS + string dependenciesDebug = ""; +#endif + unsafe { + // Note: data is a struct. It is stored on the stack and can thus not be moved by the GC. + // Therefore we do not need to pin it first. + // It is guaranteed to be stored on the stack since the Schedule method takes the data parameter by value and not by reference. + byte* dataPtr = (byte*)UnsafeUtility.AddressOf(ref data); + + var offsets = reflectionData.fieldOffsets; + for (int i = 0; i < offsets.Length; i++) { + // This is the internal value of the m_Buffer field of the NativeArray + void* nativeArrayBufferPtr = *(void**)(dataPtr + offsets[i]); + + // Use the pointer as a hash to uniquely identify a NativeArray + var hash = (long)nativeArrayBufferPtr; + + //JobDependencyAnalyzerAssociated.iteratingSlotsSampler.Begin(); + for (int j = 0; j <= slots.Count; j++) { + // No slot found. Add a new one + if (j == slots.Count) { + slots.Add(new JobDependencyTracker.NativeArraySlot { + hash = hash, + lastWrite = default, + lastReads = ListPool.Claim(), + initialized = true, // We don't know anything about the array, so assume it contains initialized data. JobDependencyTracker.NewNativeArray should be used otherwise. + hasWrite = false, + }); + } + + // Check if we know about this NativeArray yet + var slot = slots[j]; + if (slot.hash == hash) { + if (reflectionData.checkUninitializedRead[i] && !slot.initialized) { + throw new System.InvalidOperationException("A job tries to read from the native array " + typeof(T).Name + "." + reflectionData.fieldNames[i] + " which contains uninitialized data"); + } + + if (slot.hasWrite && !HasHash(dependencyHashes, slot.lastWrite.hash, numDependencies)) { + // Reads/writes always depend on the last write to the native array + dependencies[numDependencies] = slot.lastWrite.handle; + dependencyHashes[numDependencies] = slot.lastWrite.hash; + numDependencies++; + if (numDependencies >= dependencies.Length) throw new System.Exception("Too many dependencies for job"); +#if DEBUG_JOBS + dependenciesDebug += slot.lastWrite.name + " "; +#endif + } + + // If we want to write to the array we additionally depend on all previous reads of the array + if (reflectionData.writes[i]) { + for (int q = 0; q < slot.lastReads.Count; q++) { + if (!HasHash(dependencyHashes, slot.lastReads[q].hash, numDependencies)) { + dependencies[numDependencies] = slot.lastReads[q].handle; + dependencyHashes[numDependencies] = slot.lastReads[q].hash; + numDependencies++; + if (numDependencies >= dependencies.Length) throw new System.Exception("Too many dependencies for job"); +#if DEBUG_JOBS + dependenciesDebug += slot.lastReads[q].name + " "; +#endif + } + } + } + break; + } + } + //JobDependencyAnalyzerAssociated.iteratingSlotsSampler.End(); + } + + if (useAdditionalDependency) { + dependencies[numDependencies] = additionalDependency; + numDependencies++; +#if DEBUG_JOBS + dependenciesDebug += "[additional dependency]"; +#endif + } + +#if DEBUG_JOBS + UnityEngine.Debug.Log(typeof(T) + " depends on " + dependenciesDebug); +#endif + + // JobDependencyAnalyzerAssociated.getDependenciesSampler.End(); + if (numDependencies == 0) { + return default; + } else if (numDependencies == 1) { + return dependencies[0]; + } else { + //JobDependencyAnalyzerAssociated.combineSampler.Begin(); + return JobHandle.CombineDependencies(dependencies.Slice(0, numDependencies)); + //JobDependencyAnalyzerAssociated.combineSampler.End(); + } + } + } + + internal static void Scheduled (ref T data, JobDependencyTracker tracker, JobHandle job) { + unsafe { + int jobHash = JobDependencyAnalyzerAssociated.jobCounter++; + // Note: data is a struct. It is stored on the stack and can thus not be moved by the GC. + // Therefore we do not need to pin it first. + // It is guaranteed to be stored on the stack since the Schedule method takes the data parameter by value and not by reference. + byte* dataPtr = (byte*)UnsafeUtility.AddressOf(ref data); + for (int i = 0; i < reflectionData.fieldOffsets.Length; i++) { + // This is the internal value of the m_Buffer field of the NativeArray + void* nativeArrayBufferPtr = *(void**)(dataPtr + reflectionData.fieldOffsets[i]); + + // Use the pointer as a hash to uniquely identify a NativeArray + var hash = (long)nativeArrayBufferPtr; +#if DEBUG_JOBS + if (reflectionData.writes[i]) tracker.JobWritesTo(job, hash, jobHash, typeof(T).Name); + else tracker.JobReadsFrom(job, hash, jobHash, typeof(T).Name); +#else + if (reflectionData.writes[i]) tracker.JobWritesTo(job, hash, jobHash); + else tracker.JobReadsFrom(job, hash, jobHash); +#endif + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/JobDependencyTracker.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/JobDependencyTracker.cs.meta new file mode 100644 index 0000000..6cddadf --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/JobDependencyTracker.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a1d49fc8ef50f4358b3813b82043ee07 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/MeshUtility.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/MeshUtility.cs new file mode 100644 index 0000000..1800fb4 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/MeshUtility.cs @@ -0,0 +1,120 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Burst; +using Unity.Jobs; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Mathematics; + +namespace Pathfinding.Util { +#if MODULE_COLLECTIONS_2_1_0_OR_NEWER + using NativeHashMapInt3Int = Unity.Collections.NativeHashMap; +#else + using NativeHashMapInt3Int = Unity.Collections.NativeParallelHashMap; +#endif + + /// Helper class for working with meshes efficiently + [BurstCompile] + static class MeshUtility { + public static void GetMeshData (Mesh.MeshDataArray meshData, int meshIndex, out NativeArray vertices, out NativeArray indices) { + var rawMeshData = meshData[meshIndex]; + vertices = new NativeArray(rawMeshData.vertexCount, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + rawMeshData.GetVertices(vertices); + int totalIndices = 0; + for (int subMeshIndex = 0; subMeshIndex < rawMeshData.subMeshCount; subMeshIndex++) { + totalIndices += rawMeshData.GetSubMesh(subMeshIndex).indexCount; + } + indices = new NativeArray(totalIndices, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + int offset = 0; + for (int subMeshIndex = 0; subMeshIndex < rawMeshData.subMeshCount; subMeshIndex++) { + var submesh = rawMeshData.GetSubMesh(subMeshIndex); + rawMeshData.GetIndices(indices.GetSubArray(offset, submesh.indexCount), subMeshIndex); + offset += submesh.indexCount; + } + } + + /// + /// Flips triangles such that they are all clockwise in graph space. + /// + /// The triangles may not be clockwise in world space since the graphs can be rotated. + /// + /// The triangles array will be modified in-place. + /// + [BurstCompile] + public static void MakeTrianglesClockwise (ref UnsafeSpan vertices, ref UnsafeSpan triangles) { + for (int i = 0; i < triangles.Length; i += 3) { + // Make sure the triangle is clockwise in graph space (it may not be in world space since the graphs can be rotated) + // Note that we also modify the original triangle array because if the graph is cached then we will re-initialize the nodes from that array and assume all triangles are clockwise. + if (!VectorMath.IsClockwiseXZ(vertices[triangles[i+0]], vertices[triangles[i+1]], vertices[triangles[i+2]])) { + var tmp = triangles[i+0]; + triangles[i+0] = triangles[i+2]; + triangles[i+2] = tmp; + } + } + } + + /// Removes duplicate vertices from the array and updates the triangle array. + [BurstCompile] + public struct JobRemoveDuplicateVertices : IJob { + [ReadOnly] + public NativeArray vertices; + [ReadOnly] + public NativeArray triangles; + [ReadOnly] + public NativeArray tags; + + public unsafe UnsafeAppendBuffer* outputVertices; // Element Type Int3 + public unsafe UnsafeAppendBuffer* outputTriangles; // Element Type int + public unsafe UnsafeAppendBuffer* outputTags; // Element Type uint + + public static int3 cross(int3 x, int3 y) => (x * y.yzx - x.yzx * y).yzx; + + public void Execute () { + int numDegenerate = 0; + unsafe { + outputVertices->Reset(); + outputTriangles->Reset(); + outputTags->Reset(); + + var firstVerts = new NativeHashMapInt3Int(vertices.Length, Allocator.Temp); + + // Remove duplicate vertices + var compressedPointers = new NativeArray(vertices.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + int count = 0; + + for (int i = 0; i < vertices.Length; i++) { + if (firstVerts.TryAdd(vertices[i], count)) { + compressedPointers[i] = count; + outputVertices->Add(vertices[i]); + count++; + } else { + // There are some cases, rare but still there, that vertices are identical + compressedPointers[i] = firstVerts[vertices[i]]; + } + } + + for (int i = 0, j = 0; i < triangles.Length; i += 3, j++) { + var a = triangles[i+0]; + var b = triangles[i+1]; + var c = triangles[i+2]; + + // In some cases, users feed a navmesh graph a mesh with degenerate triangles. + // These are triangles with a zero area. + // We must remove these as they can otherwise cause issues for the JobCalculateTriangleConnections job, and they are generally just bad to include a navmesh. + // Note: This cross product calculation can result in overflows if the triangle is large, but since we check for equality with zero it should not be a problem in practice. + if (math.all(cross(vertices.ReinterpretLoad(b) - vertices.ReinterpretLoad(a), vertices.ReinterpretLoad(c) - vertices.ReinterpretLoad(a)) == 0)) { + // Degenerate triangle + numDegenerate++; + continue; + } + outputTriangles->Add(new int3(compressedPointers[a], compressedPointers[b], compressedPointers[c])); + outputTags->Add(tags[j]); + } + } + if (numDegenerate > 0) { + Debug.LogWarning($"Input mesh contained {numDegenerate} degenerate triangles. These have been removed.\nA degenerate triangle is a triangle with zero area. It resembles a line or a point."); + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/MeshUtility.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/MeshUtility.cs.meta new file mode 100644 index 0000000..9632829 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/MeshUtility.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f8c56d04719caa93e8baf69ed1ce8fb0 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathTracer.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathTracer.cs new file mode 100644 index 0000000..62d5e72 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathTracer.cs @@ -0,0 +1,2071 @@ +using System.Collections.Generic; +using UnityEngine; +using Pathfinding.Util; +using UnityEngine.Assertions; +using Unity.Mathematics; +using Pathfinding.Drawing; +using Unity.Collections; +using Unity.Burst; +using UnityEngine.Profiling; +using Unity.Profiling; +using Pathfinding.Graphs.Navmesh; +using System.Runtime.CompilerServices; +using Unity.Jobs.LowLevel.Unsafe; + +namespace Pathfinding { + /// + /// Helper for following a path. + /// + /// This struct keeps track of the path from the agent's current position to the end of the path. + /// Whenever the agent moves you should call to update the path. This will automatically + /// update the path if the agent has moved to the next node, or repair the path if the agent has been pushed + /// away into a node which wasn't even on the original path. + /// If the destination changes you should call to update the path. This also repairs the path + /// and it allows you to instantly get a valid path to the new destination, unless the destination has + /// changed so much that the repair is insufficient. In that case you will have to wait for the next + /// path recalculation. Check to see if the PathTracer recommends that the path be recalculated. + /// + /// After repairing the path, it will be valid, but it will not necessarily be the shortest possible path. + /// Therefore it is still recommended that you recalculate the path every now and then. + /// + /// The PathTracer stores the current path as a series of nodes. When the direction to move in is requested (by calling , + /// a path will be found which passes through all those nodes, using the funnel algorithm to simplify the path. + /// In some cases the path will contain inner vertices which make the path longer than it needs to be. Those will be + /// iteratively removed until the path is as short as possible. For performance only a limited number of iterations are performed per frame, + /// but this is usually fast enough that the simplification appears to be instant. + /// + /// Warning: This struct allocates unmanaged memory. You must call when you are done with it, to avoid memory leaks. + /// + /// Note: This is a struct, not a class. This means that if you need to pass it around, or return it from a property, you must use the ref keyword, as otherwise C# will just make a copy. + /// + /// + /// using Pathfinding; + /// using Pathfinding.Drawing; + /// using Pathfinding.Util; + /// using Unity.Collections; + /// using Unity.Mathematics; + /// using UnityEngine; + /// + /// /** Demonstrates how to use a PathTracer. + /// * + /// * The script will calculate a path to a point a few meters ahead of it, and then use the PathTracer to show the next 10 corners of the path in the scene view. + /// * If you move the object around in the scene view, you'll see the path update in real time. + /// */ + /// public class PathTracerTest : MonoBehaviour { + /// PathTracer tracer; + /// + /// /** Create a new movement plane that indicates that the agent moves in the XZ plane. + /// * This is the default for 3D games. + /// */ + /// NativeMovementPlane movementPlane => new NativeMovementPlane(Quaternion.identity); + /// ABPath lastCalculatedPath; + /// + /// void OnEnable () { + /// tracer = new PathTracer(Allocator.Persistent); + /// } + /// + /// void OnDisable () { + /// // Release all unmanaged memory from the path tracer, to avoid memory leaks + /// tracer.Dispose(); + /// } + /// + /// void Start () { + /// // Schedule a path calculation to a point ahead of this object + /// var path = ABPath.Construct(transform.position, transform.position + transform.forward*10, (p) => { + /// // This callback will be called when the path has been calculated + /// var path = p as ABPath; + /// + /// if (path.error) { + /// // The path could not be calculated + /// Debug.LogError("Could not calculate path"); + /// return; + /// } + /// + /// // Split the path into normal sequences of nodes, and off-mesh links + /// var parts = Funnel.SplitIntoParts(path); + /// + /// // Assign the path to the PathTracer + /// tracer.SetPath(parts, path.path, path.originalStartPoint, path.originalEndPoint, movementPlane, path.traversalProvider, path); + /// lastCalculatedPath = path; + /// }); + /// AstarPath.StartPath(path); + /// } + /// + /// void Update () { + /// if (lastCalculatedPath == null || !tracer.isCreated) return; + /// + /// // Repair the path to start from the transform's position + /// // If you move the transform around in the scene view, you'll see the path update in real time + /// tracer.UpdateStart(transform.position, PathTracer.RepairQuality.High, movementPlane, lastCalculatedPath.traversalProvider, lastCalculatedPath); + /// + /// // Get up to the next 10 corners of the path + /// var buffer = new NativeList(Allocator.Temp); + /// NativeArray scratchArray = default; + /// tracer.GetNextCorners(buffer, 10, ref scratchArray, Allocator.Temp, lastCalculatedPath.traversalProvider, lastCalculatedPath); + /// + /// // Draw the next 10 corners of the path in the scene view + /// using (Draw.WithLineWidth(2)) { + /// Draw.Polyline(buffer.AsArray(), Color.red); + /// } + /// + /// // Release all temporary unmanaged memory + /// buffer.Dispose(); + /// scratchArray.Dispose(); + /// } + /// } + /// + /// + [BurstCompile] + public struct PathTracer { + Funnel.PathPart[] parts; + + /// All nodes in the path + CircularBuffer nodes; + + /// + /// Hashes of some important data for each node, to determine if the node has been invalidated in some way. + /// + /// For e.g. the grid graph, this is done using the node's index in the grid. This ensures that the node is counted as invalid + /// if the node is for example moved to the other side of the graph using the . + /// + /// For all nodes, this includes if info about if the node has been destroyed, and if it is walkable. + /// + /// This will always have the same length as the array, and the absolute indices in this array will correspond to the absolute indices in the array. + /// + CircularBuffer nodeHashes; + + /// + /// Indicates if portals are definitely not inner corners, or if they may be. + /// For each portal, if bit 0 is set then the left side of the portal is definitely not an inner corner. + /// If bit 1 is set that means the same thing but for the right side of the portal. + /// + /// Should always have the same length as the portals in . + /// + CircularBuffer portalIsNotInnerCorner; + + Funnel.FunnelState funnelState; + Vector3 unclampedEndPoint; + Vector3 unclampedStartPoint; + GraphNode startNodeInternal; + + NNConstraint nnConstraint; + + int firstPartIndex; + bool startIsUpToDate; + bool endIsUpToDate; + + /// + /// If true, the first part contains destroyed nodes. + /// This can happen if the graph is updated and some nodes are destroyed. + /// + /// If this is true, the path is considered stale and should be recalculated. + /// + /// The opposite is not necessarily true. If this is false, the path may still be stale. + /// + /// See: + /// + bool firstPartContainsDestroyedNodes; + + /// + /// The type of graph that the current path part is on. + /// + /// This is either a grid-like graph, or a navmesh-like graph. + /// + public PartGraphType partGraphType; + + /// Type of graph that the current path part is on + public enum PartGraphType : byte { + /// + /// A navmesh-like graph. + /// + /// Typically either a or a + /// + Navmesh, + /// + /// A grid-like graph. + /// + /// Typically either a or a + /// + Grid, + OffMeshLink, + } + + /// Incremented whenever the path is changed + public ushort version { get; private set; } + + /// True until is called + public readonly bool isCreated => funnelState.unwrappedPortals.IsCreated; + + /// + /// Current start node of the path. + /// Since the path is updated every time the agent moves, this will be the node which the agent is inside. + /// + /// In case the path has become invalid, this will be set to the closest node to the agent, or if no such node could be found, it will be set to null. + /// + /// Note: Not necessarily up to date unless has been called first. + /// + public GraphNode startNode { + readonly get => startNodeInternal != null && !startNodeInternal.Destroyed ? startNodeInternal : null; + private set => startNodeInternal = value; + } + + /// + /// True if the path is stale and should be recalculated as quickly as possible. + /// This is true if the path has become invalid (e.g. due to a graph update), or if the destination has changed so much that we don't have a path to the destination at all. + /// + /// For performance reasons, the agent tries to avoid checking if nodes have been destroyed unless it needs to access them to calculate its movement. + /// Therefore, if a path is invalidated further ahead, the agent may not realize this until it has moved close enough. + /// + public readonly bool isStale => !endIsUpToDate || !startIsUpToDate || firstPartContainsDestroyedNodes; + + /// + /// Number of parts in the path. + /// A part is either a sequence of adjacent nodes, or an off-mesh link. + /// + public readonly int partCount => parts != null ? parts.Length - firstPartIndex : 0; + + /// True if there is a path to follow + public readonly bool hasPath => partCount > 0; + + /// Start point of the path + public readonly Vector3 startPoint => this.parts[this.firstPartIndex].startPoint; + + /// + /// End point of the path. + /// + /// This is not necessarily the same as the destination, as this point may be clamped to the graph. + /// + public readonly Vector3 endPoint => this.parts[this.parts.Length - 1].endPoint; + + /// + /// End point of the current path part. + /// + /// If the path has multiple parts, this is typically the start of an off-mesh link. + /// If the path has only one part, this is the same as . + /// + public readonly Vector3 endPointOfFirstPart => this.parts[this.firstPartIndex].endPoint; + + /// + /// The minimum number of corners to request from GetNextCornerIndices to ensure the path can be simplified well. + /// + /// The path simplification algorithm requires at least 2 corners on navmesh graphs, but 3 corners on grid graphs. + /// + public int desiredCornersForGoodSimplification => partGraphType == PartGraphType.Grid ? 3 : 2; + + /// + /// True if the next part in the path exists, and is a valid link. + /// This is true if the path has at least 2 parts and the second part is an off-mesh link. + /// + /// If any nodes in the second part have been destroyed, this will return false. + /// + public readonly bool isNextPartValidLink => partCount > 1 && GetPartType(1) == Funnel.PartType.OffMeshLink && !PartContainsDestroyedNodes(1); + + /// Create a new empty path tracer + public PathTracer(Allocator allocator) { + funnelState = new Funnel.FunnelState(16, allocator); + parts = null; + nodes = new CircularBuffer(16); + portalIsNotInnerCorner = new CircularBuffer(16); + nodeHashes = new CircularBuffer(16); + unclampedEndPoint = unclampedStartPoint = Vector3.zero; + firstPartIndex = 0; + startIsUpToDate = false; + endIsUpToDate = false; + firstPartContainsDestroyedNodes = false; + startNodeInternal = null; + version = 1; + nnConstraint = NNConstraint.Walkable; + partGraphType = PartGraphType.Navmesh; + Clear(); + } + + /// Disposes of all unmanaged memory allocated by this path tracer and resets all properties + public void Dispose () { + Clear(); + funnelState.Dispose(); + } + + public enum RepairQuality { + Low, + High + } + + /// + /// Update the start point of the path, clamping it to the graph, and repairing the path if necessary. + /// + /// This may cause to become true, if the path could not be repaired successfully. + /// + /// Returns: The new start point, which has been clamped to the graph. + /// + /// See: + /// + public Vector3 UpdateStart (Vector3 position, RepairQuality quality, NativeMovementPlane movementPlane, ITraversalProvider traversalProvider, Path path) { + Repair(position, true, quality, movementPlane, traversalProvider, path); + return parts[firstPartIndex].startPoint; + } + + /// + /// Update the end point of the path, clamping it to the graph, and repairing the path if necessary. + /// + /// This may cause to become true, if the path could not be repaired successfully. + /// + /// Returns: The new end point, which has been clamped to the graph. + /// + /// See: + /// + public Vector3 UpdateEnd (Vector3 position, RepairQuality quality, NativeMovementPlane movementPlane, ITraversalProvider traversalProvider, Path path) { + Repair(position, false, quality, movementPlane, traversalProvider, path); + return parts[parts.Length-1].endPoint; + } + + void AppendNode (bool toStart, GraphNode node) { + var partIndex = toStart ? firstPartIndex : parts.Length-1; + ref var part = ref parts[partIndex]; + var prevNode = part.endIndex >= part.startIndex ? nodes.GetBoundaryValue(toStart) : null; + + // We can ignore appending duplicate nodes + if (node == prevNode) return; + if (node == null) throw new System.ArgumentNullException(); + + if (part.endIndex >= part.startIndex + 1 && nodes.GetAbsolute(toStart ? part.startIndex + 1 : part.endIndex - 1) == node) { + // Moving from A -> B -> A can be collapsed to just A. Pop B from the current path. + if (toStart) part.startIndex++; + else part.endIndex--; + nodes.Pop(toStart); + nodeHashes.Pop(toStart); + if (partIndex == this.firstPartIndex && funnelState.leftFunnel.Length > 0) { + funnelState.Pop(toStart); + portalIsNotInnerCorner.Pop(toStart); + } + return; + } + + if (partIndex == this.firstPartIndex) { + if (prevNode != null) { + Vector3 tmpLeft; + Vector3 tmpRight; + if (toStart) { + if (!node.GetPortal(prevNode, out tmpLeft, out tmpRight)) { + throw new System.NotImplementedException(); + } + } else { + if (!prevNode.GetPortal(node, out tmpLeft, out tmpRight)) { + throw new System.NotImplementedException(); + } + } + funnelState.Push(toStart, tmpLeft, tmpRight); + portalIsNotInnerCorner.Push(toStart, 0); + } + } + nodes.Push(toStart, node); + nodeHashes.Push(toStart, HashNode(node)); + if (toStart) { + part.startIndex--; + } else { + part.endIndex++; + } + + Assert.IsTrue(part.endIndex >= part.startIndex); + } + + /// Appends the given nodes to the start or to the end of the path, one by one + void AppendPath (bool toStart, CircularBuffer path) { + if (path.Length == 0) return; + + CheckInvariants(); + + while (path.Length > 0) AppendNode(toStart, path.PopStart()); + if (toStart) { + startNode = nodes.First; + + // Check a few nodes ahead to see if any have been destroyed. + // We must do this after we have repaired the path, because repairing the path + // may remove nodes from the path (e.g. it will take the sequence A -> B -> C + [repair C -> B] and turn it into A -> B). + // Our invariants say that if firstPartContainsDestroyedNodes is true then the first part must contain destroyed nodes. + var lastDestroyCheckIndex = Mathf.Min(parts[firstPartIndex].startIndex + NODES_TO_CHECK_FOR_DESTRUCTION, parts[firstPartIndex].endIndex); + bool foundDestroyedNodes = false; + for (int i = parts[firstPartIndex].startIndex; i <= lastDestroyCheckIndex; i++) { + foundDestroyedNodes |= !ValidInPath(i); + } + firstPartContainsDestroyedNodes = foundDestroyedNodes; + } + + CheckInvariants(); + } + + /// + /// Checks that invariants are satisfied. + /// This is only called in the editor for performance reasons. + /// + /// - must be in bounds of . + /// - The first part must contain at least 1 node (unless there are no parts in the path at all). + /// - The number of nodes in the first part must be equal to the number of portals in the funnel state + 1. + /// - The number of portals in the funnel state must equal . + /// - The last node of the last part must end at the end of the path. + /// - The first node of the first part must start at the start of the path. + /// - implies that there must be at least one destroyed node in the first part (this is an implication, not an equivalence). + /// - If the first node is not destroyed, then must be the first node in the first part. + /// + [System.Diagnostics.Conditional("UNITY_EDITOR")] + void CheckInvariants () { + // Invariant checking is stripped out of the final package + } + + /// + /// Removes nodes [startIndex, startIndex+toRemove) and then inserts the given nodes at startIndex. + /// + /// Returns true if the splicing succeeded. + /// Returns false if splicing failed because it would have to access destroyed nodes. + /// In that case the path is left unmodified. + /// + /// Absolute index of the first node to remove + /// Number of nodes to remove + /// Nodes to insert at startIndex. The nodes must not be destroyed. Passing null is equivalent to an empty list. + bool SplicePath (int startIndex, int toRemove, List toInsert) { + ref var part = ref parts[firstPartIndex]; + if (startIndex < part.startIndex || startIndex + toRemove - 1 > part.endIndex) throw new System.ArgumentException("This method can only handle splicing the first part of the path"); + + // Ignore replacing nodes with the same nodes. + // This is reasonably common when the path is repaired, especially for grid graphs. + // This is mostly a performance optimization, + // however it is required for correctness in some cases. + // In particular, assume a grid graph path A B C D exists, but D is destroyed. Then if we'd try to + // splice A B C into A E C, we would try to recalculate the portal between C and D if we did + // not run this optimization. + // This is a relatively common case when doing simplifications on grid graphs. + if (toInsert != null) { + int prefix = 0; + int suffix = 0; + while (prefix < toInsert.Count && prefix < toRemove && toInsert[prefix] == nodes.GetAbsolute(startIndex + prefix)) prefix++; + + // Check if we are replacing a sequence of nodes with the same nodes + if (prefix == toInsert.Count && prefix == toRemove) return true; + + while (suffix < toInsert.Count - prefix && suffix < toRemove - prefix && toInsert[toInsert.Count - suffix - 1] == nodes.GetAbsolute(startIndex + toRemove - suffix - 1)) suffix++; + + toInsert.RemoveRange(toInsert.Count - suffix, suffix); + toInsert.RemoveRange(0, prefix); + startIndex += prefix; + toRemove -= prefix + suffix; + Assert.IsTrue(toInsert.Count >= 0); + Assert.IsTrue(toRemove >= 0); + } + + CheckInvariants(); +#if UNITY_EDITOR + if (toInsert != null) { + for (int i = 0; i < toInsert.Count; i++) { + if (!Valid(toInsert[i])) throw new System.ArgumentException("Cannot insert destroyed or unwalkable nodes"); + } + } +#endif + var numToInsert = toInsert != null ? toInsert.Count : 0; + + // We need to access the nodes next to the range we are inserting. + // If those nodes are not valid, then we cannot continue + if (startIndex - 1 >= part.startIndex && !ValidInPath(startIndex - 1)) return false; + if (startIndex + toRemove <= part.endIndex && !ValidInPath(startIndex + toRemove)) return false; + + nodes.SpliceAbsolute(startIndex, toRemove, toInsert); + nodeHashes.SpliceUninitializedAbsolute(startIndex, toRemove, numToInsert); + if (toInsert != null) { + for (int i = 0; i < toInsert.Count; i++) nodeHashes.SetAbsolute(startIndex + i, HashNode(toInsert[i])); + } + var nodesInserted = numToInsert - toRemove; + + var affectedPortalsStart1 = math.max(startIndex - 1, part.startIndex); + var affectedPortalsEnd1 = math.min(startIndex + toRemove, part.endIndex); + var portalsToRemove = affectedPortalsEnd1 - affectedPortalsStart1; + + part.endIndex += nodesInserted; + for (int j = firstPartIndex + 1; j < parts.Length; j++) { + parts[j].startIndex += nodesInserted; + parts[j].endIndex += nodesInserted; + } + + var tmpLeft = ListPool.Claim(); + var tmpRight = ListPool.Claim(); + var endIndex = startIndex + numToInsert - 1; + + // If we have a path A B C D E + // and we splice it like: A B X Y Z E + // then all portals between B|X and Z|E need to be recalculated + var affectedPortalsStart2 = math.max(startIndex - 1, part.startIndex); + var affectedPortalsEnd2 = math.min(endIndex + 1, part.endIndex); + CalculateFunnelPortals(affectedPortalsStart2, affectedPortalsEnd2, tmpLeft, tmpRight); + + funnelState.Splice(affectedPortalsStart2 - part.startIndex, portalsToRemove, tmpLeft, tmpRight); + portalIsNotInnerCorner.SpliceUninitialized(affectedPortalsStart2 - part.startIndex, portalsToRemove, tmpLeft.Count); + for (int i = 0; i < tmpLeft.Count; i++) portalIsNotInnerCorner[affectedPortalsStart2 - part.startIndex + i] = 0; + + ListPool.Release(ref tmpLeft); + ListPool.Release(ref tmpRight); + CheckInvariants(); + return true; + } + + static bool ContainsPoint (GraphNode node, Vector3 point, NativeMovementPlane plane) { + if (node is TriangleMeshNode tnode) return tnode.ContainsPoint(point, plane); + else return node.ContainsPoint(point); + } + + /// + /// Burstified function which checks if a point is inside a triangle-node and if so, projects that point onto the node's surface. + /// Returns: If the point is inside the node. + /// + [BurstCompile] + static bool ContainsAndProject (ref Int3 a, ref Int3 b, ref Int3 c, ref Vector3 p, float height, ref NativeMovementPlane movementPlane, out Vector3 projected) { + var pa = (int3)a; + var pb = (int3)b; + var pc = (int3)c; + var pp = (int3)(Int3)p; + if (!Polygon.ContainsPoint(ref pa, ref pb, ref pc, ref pp, ref movementPlane)) { + projected = Vector3.zero; + return false; + } + + var paf = (float3)(Vector3)a; + var pbf = (float3)(Vector3)b; + var pcf = (float3)(Vector3)c; + var pf = (float3)p; + var distSq = math.lengthsq(Polygon.ClosestPointOnTriangle(paf, pbf, pcf, pf) - pf); + var distThreshold = height * 0.5f; + if (distSq >= distThreshold*distThreshold) { + projected = Vector3.zero; + return false; + } + + var up = movementPlane.ToWorld(float2.zero, 1); + projected = (Vector3)ProjectOnSurface(paf, pbf, pcf, pf, up); + return true; + } + + static float3 ProjectOnSurface (float3 a, float3 b, float3 c, float3 p, float3 up) { + var nodeNormal = math.cross(c - a, b - a); + var dot = math.dot(nodeNormal, up); + if (math.abs(dot) > math.FLT_MIN_NORMAL) { + var w = p - a; + var fac = -math.dot(nodeNormal, w) / dot; + return p + fac * up; + } else { + // Node plane is perpendicular to the movement plane + return p; + } + } + + void Repair (Vector3 point, bool isStart, RepairQuality quality, NativeMovementPlane movementPlane, ITraversalProvider traversalProvider, Path path, bool allowCache = true) { + int partIndex; + GraphNode currentNode; + bool samePoint; + int currentIndexInPath; + if (isStart) { + partIndex = this.firstPartIndex; + currentIndexInPath = parts[partIndex].startIndex; + currentNode = nodes.GetAbsolute(currentIndexInPath); + samePoint = unclampedStartPoint == point; + } else { + partIndex = this.parts.Length - 1; + currentIndexInPath = parts[partIndex].endIndex; + currentNode = nodes.GetAbsolute(currentIndexInPath); + samePoint = unclampedEndPoint == point; + } + + // Early return in case nothing has changed since last time + // Note that the point may be outside the current node, in which case we should + // keep the startIsUpToDate/endIsUpToDate flags as they are. + // If the path is currently stale, it is possible that doing a full repair again + // would find a better node (possibly even make it not stale anymore), but it is + // common that this would just lead to wasted computations, so we don't. + bool currentNodeValid = ValidInPath(currentIndexInPath); + if (allowCache && samePoint && currentNodeValid) { + return; + } + + if (partGraphType == PartGraphType.OffMeshLink) throw new System.InvalidOperationException("Cannot repair path while on an off-mesh link"); + + ref var part = ref parts[partIndex]; + + if (!float.IsFinite(point.x)) { + if (isStart) throw new System.ArgumentException("Position must be a finite vector"); + else { + unclampedEndPoint = point; + endIsUpToDate = false; + // Remove all nodes except the first one from the path + RemoveAllPartsExceptFirst(); + ref var firstPart = ref parts[firstPartIndex]; + if (firstPart.endIndex > firstPart.startIndex) SplicePath(firstPart.startIndex + 1, firstPart.endIndex - firstPart.startIndex, null); + firstPart.endPoint = firstPart.startPoint; + version++; + + CheckInvariants(); + return; + } + } + + const float height = 1.0f; + if (currentNodeValid) { + // Check if we are inside the same node as last frame. This is the common case. + // We also ensure that we are not too far above the node as that might indicate + // that we have been teleported to a point with the same XZ coordinates, + // but for instance on a different floor of a building. + // If so, we just project the position on the node's surface and return. + bool insideCurrentNode = false; + Vector3 newClampedPoint = Vector3.zero; + if (currentNode is TriangleMeshNode tnode) { + tnode.GetVertices(out var a, out var b, out var c); + insideCurrentNode = ContainsAndProject(ref a, ref b, ref c, ref point, height, ref movementPlane, out newClampedPoint); + } else if (currentNode is GridNodeBase gnode) { + // TODO: Can be optimized + // TODO: Also check for height + if (gnode.ContainsPoint(point)) { + insideCurrentNode = true; + newClampedPoint = gnode.ClosestPointOnNode(point); + } + } + + if (insideCurrentNode) { + if (isStart) { + part.startPoint = newClampedPoint; + unclampedStartPoint = point; + startIsUpToDate = true; + // Setting startIsUpToDate may have made the path non-stale, and thus we may have to update the startNode + // to preserve our invariants (see CheckInvariants). + startNode = currentNode; + } else { + part.endPoint = newClampedPoint; + unclampedEndPoint = point; + endIsUpToDate = true; + } + version++; + CheckInvariants(); + return; + } + } + + // Split out into a separate call because otherwise the lambda inside that function + // will cause an allocation even if we make an early return before we even use it. + RepairFull(point, isStart, quality, movementPlane, traversalProvider, path); + version++; + CheckInvariants(); + } + + private static readonly ProfilerMarker MarkerContains = new ProfilerMarker("ContainsNode"); + private static readonly ProfilerMarker MarkerClosest = new ProfilerMarker("ClosestPointOnNode"); + private static readonly ProfilerMarker MarkerGetNearest = new ProfilerMarker("GetNearest"); + const int NODES_TO_CHECK_FOR_DESTRUCTION = 5; + + /// + /// Use a heuristic to determine when an agent has passed a portal and we need to pop it. + /// + /// Assumes the start point/end point of the first part is point, and simplifies the funnel + /// accordingly. It uses the cached portals to determine if the agent has passed a portal. + /// This works even if nodes have been destroyed. + /// + /// Note: Does not update the start/end point of the first part. + /// + void HeuristicallyPopPortals (bool isStartOfPart, Vector3 point) { + ref var part = ref parts[firstPartIndex]; + if (isStartOfPart) { + while (funnelState.IsReasonableToPopStart(point, part.endPoint)) { + part.startIndex++; + nodes.PopStart(); + nodeHashes.PopStart(); + funnelState.PopStart(); + portalIsNotInnerCorner.PopStart(); + } + if (ValidInPath(nodes.AbsoluteStartIndex)) startNode = nodes.First; + } else { + var removed = 0; + while (funnelState.IsReasonableToPopEnd(part.startPoint, point)) { + part.endIndex--; + removed++; + funnelState.PopEnd(); + portalIsNotInnerCorner.PopEnd(); + } + if (removed > 0) { + nodes.SpliceAbsolute(part.endIndex + 1, removed, null); + nodeHashes.SpliceAbsolute(part.endIndex + 1, removed, null); + for (int i = firstPartIndex + 1; i < parts.Length; i++) { + parts[i].startIndex -= removed; + parts[i].endIndex -= removed; + } + } + } + + // Check a few nodes ahead to see if any have been destroyed + var lastDestroyCheckIndex = Mathf.Min(part.startIndex + NODES_TO_CHECK_FOR_DESTRUCTION, part.endIndex); + bool foundDestroyedNodes = false; + for (int i = part.startIndex; i <= lastDestroyCheckIndex; i++) { + foundDestroyedNodes |= !ValidInPath(i); + } + + // We may end up popping all destroyed nodes from the part. + // In that case, we must set firstPartContainsDestroyedNodes to false + // to satisfy our invariants. + this.firstPartContainsDestroyedNodes = foundDestroyedNodes; + CheckInvariants(); + } + + [System.Diagnostics.Conditional("UNITY_ASSERTIONS")] + void AssertValidInPath (int absoluteNodeIndex) { + Assert.IsTrue(ValidInPath(absoluteNodeIndex)); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + readonly bool ValidInPath (int absoluteNodeIndex) { + return HashNode(nodes.GetAbsolute(absoluteNodeIndex)) == nodeHashes.GetAbsolute(absoluteNodeIndex); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static bool Valid(GraphNode node) => !node.Destroyed && node.Walkable; + + /// + /// Returns a hash with the most relevant information about a node. + /// + /// See: + /// + [MethodImpl(MethodImplOptions.AggressiveInlining)] + static int HashNode (GraphNode node) { + // Note: The node index will change if the node is destroyed + int h = (int)node.NodeIndex; + h ^= node.Walkable ? 100663319 : 0; + if (node is GridNodeBase gnode) { + h ^= gnode.NodeInGridIndex * 25165843; + } + return h; + } + + void RepairFull (Vector3 point, bool isStart, RepairQuality quality, NativeMovementPlane movementPlane, ITraversalProvider traversalProvider, Path path) { + // TODO: Rewrite to use Int3 coordinates everywhere + var maxNodesToSearch = quality == RepairQuality.High ? 16 : 9; + var partIndex = isStart ? this.firstPartIndex : this.parts.Length - 1; + ref var part = ref parts[partIndex]; + var currentIndexInPath = isStart ? part.startIndex : part.endIndex; + + // TODO + // Restructure code so that we do the heuristic passed-portal check if the next portal is destroyed (current or next node is destroyed), + // but we allow using the normal repair code if the current node exists, but the next node is destroyed. + // We only search for the globally closest node if the current node is destroyed after the heuristic passed-portal check is done. + // !! + var nextPortalDestroyed = !ValidInPath(currentIndexInPath) || (part.endIndex != part.startIndex && !ValidInPath(isStart ? part.startIndex + 1 : part.endIndex - 1)); + + if (nextPortalDestroyed && partIndex == firstPartIndex) { + // If the current node or the next node is destroyed, we still need to keep a funnel that we can use for directions + // while a new path is being calculated. + // Even though the regular repair would work if the next node is destroyed, it would + // not be able to simplify the path appropriately. + // Take the following example: + // + // - The path is A -> B -> C (agent is at node A). + // - Node B is destroyed and is replaced by a new node B' in roughly the same location + // - The agent moves into node B'. + // - If we used the normal repair code, the path would become B' -> A -> B -> C. + // This would cause the agent to turn around and go back to node A, and it would not look great. + // Instead, we heuristically pop the portal between A and B, and the path becomes B -> C. + // - Now the current node is B which is destroyed, so we enter another IF-statement below. + HeuristicallyPopPortals(isStart, point); + currentIndexInPath = isStart ? part.startIndex : part.endIndex; + } + + if (!ValidInPath(currentIndexInPath)) { + // If the current node is destroyed, we must use a global GetNearest check to find the closest non-destroyed node. + // We do this so that we can still clamp the agent to the navmesh even if the current node is destroyed. + // We do not want to replace the path with just a single node, because that may cause odd movement for a few frames + // while the path is being recalculated. So we only adjust the start point of th path, but keep following the + // potentially stale path until the new path is ready. + + if (isStart) { + firstPartContainsDestroyedNodes = true; + unclampedStartPoint = point; + startIsUpToDate = false; + + var nn = this.nnConstraint; + nn.distanceMetric = DistanceMetric.ClosestAsSeenFromAboveSoft(movementPlane.ToWorld(float2.zero, 1)); + var globallyClosestNode = AstarPath.active != null? AstarPath.active.GetNearest(point, nn).node : null; + + // Filter using the traveral provider. + // TODO: We need to change the APIs to allow the GetNearest method to use the traversal provider + if (traversalProvider != null && !traversalProvider.CanTraverse(path, globallyClosestNode)) globallyClosestNode = null; + + startNode = globallyClosestNode; + if (startNode != null) { + part.startPoint = startNode.ClosestPointOnNode(point); + + if (part.endIndex - part.startIndex < 10 && partCount <= 1) { + // This is a short path, a local repair should probably be able to repair it. + // The path will be repaired when the end of the path is being set (typically it is done right after setting the start point). + + // It is important to handle this case. Take the following example: + // The agent is standing still, but a dynamic obstacle has just been created right on + // top of the agent. If we continued following the stale path, it might be completely incorrect + // and lead into the obstacle. + // We do not do this for long paths, because the repair is unlikely to succeed, and we may end + // up with a partial path that goes in the wrong direction. It is then better to just wait for + // the path to be recalculated. + + var clampedStartPoint = part.startPoint; + // Make the path into just a single node + this.Clear(); + startNode = globallyClosestNode; + this.partGraphType = PartGraphTypeFromNode(startNode); + unclampedStartPoint = point; + unclampedEndPoint = clampedStartPoint; + this.nodes.PushEnd(globallyClosestNode); + this.nodeHashes.PushEnd(HashNode(globallyClosestNode)); + this.parts = new Funnel.PathPart[1]; + this.parts[0] = new Funnel.PathPart { + startIndex = nodes.AbsoluteStartIndex, + endIndex = nodes.AbsoluteEndIndex, + startPoint = clampedStartPoint, + endPoint = clampedStartPoint, + }; + } + } else { + part.startPoint = point; + } + } else { + // We don't care as much about the end point of the path being clamped to the navmesh. + // But we do mark the path as stale so that it will be recalculated. + unclampedEndPoint = point; + part.endPoint = point; + endIsUpToDate = false; + } + CheckInvariants(); + } else { + var repairPath = LocalSearch(nodes.GetAbsolute(currentIndexInPath), point, maxNodesToSearch, movementPlane, isStart, traversalProvider, path); + + { + // When we repair the path we may have multiple different cases: + // + // 1. We find the actually globally closest node and can repair the current path to reach it. This is the common case, and also the easiest to handle. + // + // 2. We find the globally closest node, but the destination is not actually inside it (it might be slightly inside an obstacle). + // This is fine. We just repair to the closest point that we can reach. + // + // 3. We do not find a repair path to the globally closest node. + // In this case the globally closest node might be very far away, so the path is stale. + // Or, we cannot actually reach the globally closest node due to e.g. a tag which we cannot traverse. + // In the latter case, we don't want to mark the path as stale every single frame. So we use a heuristic. + // + // Let us call the distance from a path's endpoint to the path's destination as its error. + // Typically this will be zero if the destination is on the navmesh, but may be positive if the destination is inside an obstacle or it cannot be reached. + // We say that the path is up to date if the repaired path's error is smaller or equal to the previous path's error (+ a small margin) and the previous end point was up to date. + // The same logic applies to the start point. + + var closestNode = repairPath.Last; + Assert.IsTrue(Valid(closestNode)); + + bool upToDate; + Vector3 newClampedPoint; + + var nn = this.nnConstraint; + nn.constrainArea = true; + nn.area = (int)closestNode.Area; + + MarkerGetNearest.Begin(); + var globallyClosestNode = AstarPath.active.GetNearest(point, nn); + nn.constrainArea = false; + + + MarkerGetNearest.End(); + var oldClampedPoint = isStart ? part.startPoint : part.endPoint; + // TODO: Check if distance to globally closest node is approximately the same instead, to avoid cases when two nodes are the same distance from the point + if (globallyClosestNode.node == closestNode) { + upToDate = true; + newClampedPoint = globallyClosestNode.position; + } else { + var oldUnclampedPoint = isStart ? unclampedStartPoint : unclampedEndPoint; + var oldError = (oldUnclampedPoint - oldClampedPoint).sqrMagnitude; + var oldIsUpToDate = isStart ? startIsUpToDate : endIsUpToDate; + MarkerClosest.Begin(); + newClampedPoint = closestNode.ClosestPointOnNode(point); + MarkerClosest.End(); + var newError = (point - newClampedPoint).sqrMagnitude; + + // true => Ok. The node we have found is as good, or almost as good as the actually closest node + // false => Bad. We didn't find the best node when repairing. We may need to recalculate our path. + upToDate = oldIsUpToDate && newError <= oldError + 0.1f*0.1f; + } + + // In case we did not find a good repair path, we don't update our path. + // This is important especially in the following case: + // + // wall + // v + // - - ----|---------- + // B | X + // - - ----|---------- + // + // If X is the agent, and we try to set its destination to B, + // then we want it to wait until its path calculation is done and then + // realize it has to move right to get around the wall and eventually reach B. + // If we would update the path here, the agent would first move left, thinking that + // this is the best it could repair the path, and then move right once its path + // had been recalculated. This looks like a weird stutter and is not desirable. + if (!upToDate && !isStart) { + repairPath.Clear(); + newClampedPoint = oldClampedPoint; + } + + AppendPath(isStart, repairPath); + repairPath.Pool(); + + if (isStart) { + startNode = nodes.First; + // upToDate &= !foundDestroyedNodes; + } + + if (isStart) { + const bool CLAMP = true; + if (CLAMP) { + unclampedStartPoint = point; + part.startPoint = newClampedPoint; + startIsUpToDate = true; + } else { + #pragma warning disable CS0162 + unclampedStartPoint = point; + part.startPoint = newClampedPoint; + startIsUpToDate = upToDate; + } + } else { + unclampedEndPoint = point; + part.endPoint = newClampedPoint; + endIsUpToDate = upToDate; + } + } + } + } + + /// Calculates the squared distance from a point to the closest point on the node. + /// The node to calculate the distance to + /// The point to calculate the distance from. For navmesh/recast/grid graphs this point should be in graph space. + /// Parameters for the projection, if the node is a triangle mesh node. The projection should be based on the node's graph. + static float SquaredDistanceToNode (GraphNode node, Vector3 point, ref BBTree.ProjectionParams projectionParams) { + if (node is TriangleMeshNode tnode) { + tnode.GetVerticesInGraphSpace(out var v1, out var v2, out var v3); + Polygon.ClosestPointOnTriangleProjected( + ref v1, + ref v2, + ref v3, + ref projectionParams, + ref Unity.Collections.LowLevel.Unsafe.UnsafeUtility.As(ref point), + out _, + out var dist, + out _ + ); + return dist; + } else if (node is GridNodeBase gnode) { + var c = gnode.CoordinatesInGrid; + var xf = math.clamp(point.x, c.x, c.x + 1); + var zf = math.clamp(point.z, c.y, c.y + 1); + return math.lengthsq(new float3(xf, 0, zf) - (float3)point); + } else { + var closestOnNode = node.ClosestPointOnNode(point); + return (point - closestOnNode).sqrMagnitude; + } + } + + struct QueueItem { + public GraphNode node; + public int parent; + public float distance; + } + + static bool QueueHasNode (QueueItem[] queue, int count, GraphNode node) { + for (int i = 0; i < count; i++) if (queue[i].node == node) return true; + return false; + } + +#if UNITY_2022_3_OR_NEWER + static readonly QueueItem[][] TempQueues = new QueueItem[JobsUtility.ThreadIndexCount][]; + static readonly List[] TempConnectionLists = new List[JobsUtility.ThreadIndexCount]; + + void GetTempQueue (out QueueItem[] queue, out List connections) { + var threadIndex = JobsUtility.ThreadIndex; + queue = TempQueues[threadIndex]; + connections = TempConnectionLists[threadIndex]; + if (queue == null) { + queue = TempQueues[threadIndex] = new QueueItem[16]; + connections = TempConnectionLists[threadIndex] = new List(); + } + } +#else + void GetTempQueue (out QueueItem[] queue, out List connections) { + queue = new QueueItem[16]; + connections = new List(); + } +#endif + + /// + /// Searches from currentNode until it finds a node that contains the given point. + /// + /// The return value is a list of nodes that start with currentNode and ends with the node that contains the given point, if it could be found. + /// Otherwise, the return value will be an empty list. + /// + CircularBuffer LocalSearch (GraphNode currentNode, Vector3 point, int maxNodesToSearch, NativeMovementPlane movementPlane, bool reverse, ITraversalProvider traversalProvider, Path path) { + var nn = this.nnConstraint; + nn.distanceMetric = DistanceMetric.ClosestAsSeenFromAboveSoft(movementPlane.up); + + // Grab a temporary queue and list to use for this thread + // To improve performance, we don't want to allocate these every time this method is called, + // and even the ArrayPool and ListPool classes add some overhead. + GetTempQueue(out var queue, out var connections); + Assert.IsTrue(queue.Length >= maxNodesToSearch); + + var queueHead = 0; + var queueTail = 0; + var graph = currentNode.Graph; + BBTree.ProjectionParams projectionParams; + Vector3 pointInGraphSpace; + if (partGraphType == PartGraphType.Navmesh) { + var navmeshGraph = graph as NavmeshBase; + projectionParams = new BBTree.ProjectionParams(nn, navmeshGraph.transform); + // Convert the point to graph space + pointInGraphSpace = navmeshGraph.transform.InverseTransform(point); + } else if (partGraphType == PartGraphType.Grid) { + projectionParams = default; + pointInGraphSpace = (graph as GridGraph).transform.InverseTransform(point); + } else { + projectionParams = default; + pointInGraphSpace = point; + } + float bestDist = SquaredDistanceToNode(currentNode, pointInGraphSpace, ref projectionParams); + queue[0] = new QueueItem { node = currentNode, parent = -1, distance = bestDist }; + queueTail++; + int bestNode = 0; + + while (queueHead < queueTail) { + var nodeQueueIndex = queueHead; + var node = queue[nodeQueueIndex].node; + queueHead++; + Assert.IsTrue(Valid(node)); + MarkerContains.Begin(); + + if (ContainsPoint(node, point, movementPlane)) { + MarkerContains.End(); + bestDist = 0; + bestNode = nodeQueueIndex; + break; + } else { + MarkerContains.End(); + var dist = queue[nodeQueueIndex].distance; + if (dist < bestDist) { + bestDist = dist; + bestNode = nodeQueueIndex; + } + + // Check if the neighbour nodes are closer than the parent node. + // Allow for a small margin to both avoid floating point errors and to allow + // moving past very small local minima. + var distanceThresholdSqr = dist*(1.05f*1.05f) + 0.05f; + // TODO: For grid graphs we ideally want to get only the axis-aligned connections. + // This is because otherwise we can use a diagonal connection that cannot be simplified to + // two axis-aligned connections, which will make the RemoveGridPathDiagonals method fail. + // This will only happen in very few games, but it is still a potential issue. + node.GetConnections((GraphNode node, ref List ls) => ls.Add(node), ref connections); + + for (int i = 0; i < connections.Count; i++) { + var neighbour = connections[i]; + if (queueTail < maxNodesToSearch && neighbour.GraphIndex == node.GraphIndex && nn.Suitable(neighbour) && (traversalProvider == null || (reverse ? traversalProvider.CanTraverse(path, neighbour) && traversalProvider.CanTraverse(path, neighbour, node) : traversalProvider.CanTraverse(path, node, neighbour)))) { + MarkerClosest.Begin(); + float distanceToNeighbourSqr = SquaredDistanceToNode(neighbour, pointInGraphSpace, ref projectionParams); + MarkerClosest.End(); + if (distanceToNeighbourSqr < distanceThresholdSqr && !QueueHasNode(queue, queueTail, neighbour)) { + queue[queueTail] = new QueueItem { node = neighbour, parent = nodeQueueIndex, distance = distanceToNeighbourSqr }; + queueTail++; + } + } + } + connections.Clear(); + } + } + + // Trace the repair path back to connect to the current path + var repairPath = new CircularBuffer(8); + while (bestNode != -1) { + repairPath.PushStart(queue[bestNode].node); + bestNode = queue[bestNode].parent; + } + + // Clear node references, to not leave any references that prevent garbage collection + connections.Clear(); + for (int i = 0; i < queueTail; i++) queue[i].node = null; + + if (partGraphType == PartGraphType.Grid) { + // To make the path easier to handle, we replace all diagonals by two axis-aligned connections + var hashes = new CircularBuffer(); + RemoveGridPathDiagonals(null, 0, ref repairPath, ref hashes, nnConstraint, traversalProvider, path); + } + return repairPath; + } + + /// Renders the funnel for debugging purposes. + /// The command builder to use for drawing. + /// The movement plane of the agent. + public void DrawFunnel (CommandBuilder draw, NativeMovementPlane movementPlane) { + if (parts == null) return; + var part = parts[firstPartIndex]; + funnelState.PushStart(part.startPoint, part.startPoint); + funnelState.PushEnd(part.endPoint, part.endPoint); + using (draw.WithLineWidth(2)) { + draw.Polyline(funnelState.leftFunnel); + draw.Polyline(funnelState.rightFunnel); + } + if (funnelState.unwrappedPortals.Length > 1) { + using (draw.WithLineWidth(1)) { + var up = movementPlane.up; + var m = funnelState.UnwrappedPortalsToWorldMatrix(up); + // Convert from 4x3 matrix to 4x4 matrix + var m2 = new float4x4(m.c0, m.c1, new float4(0, 0, 1, 0), m.c2); + + using (draw.WithMatrix(m2)) { + var prevLeft = funnelState.unwrappedPortals[0].xy; + var prevRight = funnelState.unwrappedPortals[1].xy; + for (int i = 0; i < funnelState.unwrappedPortals.Length; i++) { + var left = funnelState.unwrappedPortals[i].xy; + var right = funnelState.unwrappedPortals[i].zw; + draw.xy.Line(left, right, Palette.Colorbrewer.Set1.Brown); + draw.xy.Line(prevLeft, left, Palette.Colorbrewer.Set1.Brown); + draw.xy.Line(prevRight, right, Palette.Colorbrewer.Set1.Brown); + prevLeft = left; + prevRight = right; + } + } + } + } + + using (draw.WithColor(new Color(0, 0, 0, 0.2f))) { + for (int i = 0; i < funnelState.leftFunnel.Length - 1; i++) { + draw.SolidTriangle(funnelState.leftFunnel[i], funnelState.rightFunnel[i], funnelState.rightFunnel[i+1]); + draw.SolidTriangle(funnelState.leftFunnel[i], funnelState.leftFunnel[i+1], funnelState.rightFunnel[i+1]); + } + } + + using (draw.WithColor(new Color(0, 0, 1, 0.5f))) { + for (int i = 0; i < funnelState.leftFunnel.Length; i++) { + draw.Line(funnelState.leftFunnel[i], funnelState.rightFunnel[i]); + } + } + + funnelState.PopStart(); + funnelState.PopEnd(); + } + + static Int3 MaybeSetYZero (Int3 p, bool setYToZero) { + if (setYToZero) p.y = 0; + return p; + } + + static bool IsInnerVertex (CircularBuffer nodes, Funnel.PathPart part, int portalIndex, bool rightSide, List alternativeNodes, NNConstraint nnConstraint, out int startIndex, out int endIndex, ITraversalProvider traversalProvider, Path path) { + Assert.IsTrue(portalIndex >= part.startIndex && portalIndex < part.endIndex); + var startNode = nodes.GetAbsolute(portalIndex); + if (startNode is TriangleMeshNode) { + return IsInnerVertexTriangleMesh(nodes, part, portalIndex, rightSide, alternativeNodes, nnConstraint, out startIndex, out endIndex, traversalProvider, path); + } else if (startNode is GridNodeBase) { + return IsInnerVertexGrid(nodes, part, portalIndex, rightSide, alternativeNodes, nnConstraint, out startIndex, out endIndex, traversalProvider, path); + } else { + startIndex = portalIndex; + endIndex = portalIndex+1; + return false; + } + } + + static bool IsInnerVertexGrid (CircularBuffer nodes, Funnel.PathPart part, int portalIndex, bool rightSide, List alternativeNodes, NNConstraint nnConstraint, out int startIndex, out int endIndex, ITraversalProvider traversalProvider, Path path) { + startIndex = portalIndex; + endIndex = portalIndex+1; + return false; + + startIndex = portalIndex; + endIndex = portalIndex+1; + + var startNode = nodes.GetAbsolute(startIndex) as GridNodeBase; + var endNode = nodes.GetAbsolute(endIndex) as GridNodeBase; + + if (startNode == null || endNode == null || !Valid(startNode) || !Valid(endNode)) return false; + + var startX = startNode.XCoordinateInGrid; + var startZ = startNode.ZCoordinateInGrid; + var dx = endNode.XCoordinateInGrid - startX; + var dz = endNode.ZCoordinateInGrid - startZ; + + Assert.IsTrue(dx >= -1 && dx <= 1); + Assert.IsTrue(dz >= -1 && dz <= 1); + + if (dx != 0 && dz != 0) { + // TODO: Handle diagonals + return false; + } + + + var dir = GridNodeBase.OffsetToConnectionDirection(dx, dz); + // Both of these values will be either -1 or +1. + var vertexDx = GridGraph.neighbourXOffsets[dir] + GridGraph.neighbourXOffsets[(dir + (rightSide ? -1 : 1) + 4) % 4]; + var vertexDz = GridGraph.neighbourZOffsets[dir] + GridGraph.neighbourZOffsets[(dir + (rightSide ? -1 : 1) + 4) % 4]; + + while (startIndex > part.startIndex) { + var n = nodes.GetAbsolute(startIndex-1) as GridNodeBase; + if (n == null || !Valid(n)) break; + + var x = n.XCoordinateInGrid; + var z = n.ZCoordinateInGrid; + if ((x == startX || x == startX + vertexDx) && (z == startZ || z == startZ + vertexDz)) { + startIndex--; + } else { + break; + } + } + + while (endIndex < part.endIndex) { + var n = nodes.GetAbsolute(endIndex+1) as GridNodeBase; + if (n == null || !Valid(n)) break; + + var x = n.XCoordinateInGrid; + var z = n.ZCoordinateInGrid; + if ((x == startX || x == startX + vertexDx) && (z == startZ || z == startZ + vertexDz)) { + endIndex++; + } else { + break; + } + } + + if (endIndex <= startIndex+1) { + // Strange, there's a corner, but only 2 nodes that share this corner vertex. + Debug.LogError("Strange"); + return false; + } + + startNode = nodes.GetAbsolute(startIndex) as GridNodeBase; + endNode = nodes.GetAbsolute(endIndex) as GridNodeBase; + + alternativeNodes.Add(startNode); + + // The path may go around the vertex in a full loop. How silly! Let's fix that. + if (startNode == endNode) return true; + + startX = startNode.XCoordinateInGrid; + startZ = startNode.ZCoordinateInGrid; + dx = endNode.XCoordinateInGrid - startX; + dz = endNode.ZCoordinateInGrid - startZ; + if (dx == 0 || dz == 0) { + // Axis-aligned connection + if (!startNode.ContainsOutgoingConnection(endNode)) return false; + + alternativeNodes.Add(endNode); + return true; + } else { + // Diagonal connection. + // Split this into two axis-aligned connections. + // But they should go in the opposite direction around the vertex compared to the original + + var nextDir = (GridNodeBase.OffsetToConnectionDirection(dx, dz) + (rightSide ? 0 : 1)) % 4; + + var middleNode = startNode.GetNeighbourAlongDirection(nextDir); + if (middleNode == null) return false; + + if (!middleNode.ContainsOutgoingConnection(endNode)) return false; + + alternativeNodes.Add(middleNode); + alternativeNodes.Add(endNode); + return true; + } + } + + static bool IsInnerVertexTriangleMesh (CircularBuffer nodes, Funnel.PathPart part, int portalIndex, bool rightSide, List alternativeNodes, NNConstraint nnConstraint, out int startIndex, out int endIndex, ITraversalProvider traversalProvider, Path path) { + startIndex = portalIndex; + endIndex = portalIndex+1; + + var startNode = nodes.GetAbsolute(startIndex) as TriangleMeshNode; + var endNode = nodes.GetAbsolute(endIndex) as TriangleMeshNode; + + if (startNode == null || endNode == null || !Valid(startNode) || !Valid(endNode)) return false; + + if (!startNode.GetPortalInGraphSpace(endNode, out Int3 left, out Int3 right, out _, out _)) return false; + + var graph = startNode.Graph as NavmeshBase; + bool allowVertexYDifferences = graph.RecalculateNormals; + + // We ignore any difference in graph-space y coordinates. + // This is because if two vertices are on different tiles, they may have slightly different y coordinates, even if they are semantically + // the same vertex. In particular, this easily becomes the case when using a NavmeshPrefab component. + var vertex = MaybeSetYZero(rightSide ? right : left, allowVertexYDifferences); + + // Find the first and last node which shares the same portal vertex. + while ( + startIndex > part.startIndex && + nodes.GetAbsolute(startIndex-1) is TriangleMeshNode prevNode && + Valid(prevNode) && + prevNode.GetPortalInGraphSpace(startNode, out Int3 left2, out Int3 right2, out _, out _) && + MaybeSetYZero(rightSide ? right2 : left2, allowVertexYDifferences) == vertex + ) { + startNode = prevNode; + startIndex--; + } + while ( + endIndex < part.endIndex && + nodes.GetAbsolute(endIndex+1) is TriangleMeshNode nextNode && + Valid(nextNode) && + endNode.GetPortalInGraphSpace(nextNode, out Int3 left2, out Int3 right2, out _, out _) && + MaybeSetYZero(rightSide ? right2 : left2, allowVertexYDifferences) == vertex + ) { + endNode = nextNode; + endIndex++; + + // The path may go around the vertex in more than a full loop. That's very silly. + // We break here to cut out the whole loop. + // This has been observed in the "ITraversalProvider DirectionalTags" unit test. + // In particular, if we start with the following situation: + // + // 4 nodes meet at a vertex. The nodes are named A, B, C, and D. + // A path goes through A -> B -> C -> D. + // + // │ + // ──D◄───┼───┐C + // │ ▲ + // │ │ + // ───────┼───┼─── + // │ │ + // A───┼──►┘B + // │ + // + // The agent moves from A to C over a single frame, the LocalSearch finds a repair path C -> D -> A. + // This results in the path C -> D -> A -> B -> C -> D, which goes around the vertex more than a full loop. + // We want to cut out the whole loop and simplify this path to C -> D. + // + // │ + // ──D►───┼───┐C2 + // ┌───┼─C1│ + // │ │ │ + // ───┼───┼───┼── + // ▼ │ │ + // A───┼─►─┘B + // │ + if (endNode == startNode) break; + } + + var currentNode = startNode; + int cnt = 0; + alternativeNodes.Add(startNode); + + // The path may go around the vertex in a full loop. How silly! Let's fix that. + if (startNode == endNode) return true; + + while (true) { + bool found = false; + for (int j = 0; j < currentNode.connections.Length; j++) { + if (currentNode.connections[j].node is TriangleMeshNode neighbour && (traversalProvider != null ? traversalProvider.CanTraverse(path, currentNode, neighbour) : nnConstraint.Suitable(neighbour)) && currentNode.connections[j].isOutgoing) { + if (!currentNode.GetPortalInGraphSpace(neighbour, out var left2, out var right2, out _, out _)) continue; + var candidateVertex = MaybeSetYZero(rightSide ? left2 : right2, allowVertexYDifferences); + + if (candidateVertex == vertex) { + // Found a portal which shares the correct vertex + // We try to follow it and see where we end up + currentNode = neighbour; + alternativeNodes.Add(currentNode); + + // Return if we have found an alternative path + if (currentNode == endNode) return true; + + if (cnt++ > 100) throw new System.Exception("Caught in a potentially infinite loop. The navmesh probably contains degenerate geometry."); + found = true; + break; + } + } + } + if (!found) return false; + } + } + + bool FirstInnerVertex (NativeArray indices, int numCorners, List alternativePath, out int alternativeStartIndex, out int alternativeEndIndex, ITraversalProvider traversalProvider, Path path) { + var part = parts[firstPartIndex]; + Assert.AreEqual(funnelState.leftFunnel.Length, portalIsNotInnerCorner.Length); + + for (int i = 0; i < numCorners; i++) { + var idx = indices[i]; + var rightSide = (idx & Funnel.RightSideBit) != 0; + var portalIndex = idx & Funnel.FunnelPortalIndexMask; + Assert.IsTrue(portalIndex >= 0 && portalIndex < part.endIndex - part.startIndex); + if ((portalIsNotInnerCorner[portalIndex] & (rightSide ? 0b01 : 0b10)) != 0) { + continue; + } + + alternativePath.Clear(); + if (IsInnerVertex(nodes, part, part.startIndex + portalIndex, rightSide, alternativePath, nnConstraint, out alternativeStartIndex, out alternativeEndIndex, traversalProvider, path)) { + return true; + } else { + // Mark this corner as already tested + portalIsNotInnerCorner[portalIndex] = (byte)(portalIsNotInnerCorner[portalIndex] | (rightSide ? 0b01 : 0b10)); + } + } + + alternativeStartIndex = -1; + alternativeEndIndex = -1; + return false; + } + + /// + /// Estimates the remaining distance to the end of the current path part. + /// + /// Note: This method may modify the internal PathTracer state, so it is not safe to call it from multiple threads at the same time. + /// + public float EstimateRemainingPath (int maxCorners, ref NativeMovementPlane movementPlane) { + return EstimateRemainingPath(ref funnelState, ref parts[firstPartIndex], maxCorners, ref movementPlane); + } + + [BurstCompile] + static float EstimateRemainingPath (ref Funnel.FunnelState funnelState, ref Funnel.PathPart part, int maxCorners, ref NativeMovementPlane movementPlane) { + var buffer = new NativeList(maxCorners, Allocator.Temp); + var cornerIndices = new NativeArray(maxCorners, Allocator.Temp); + + // Treat start point as a corner + maxCorners -= 1; + maxCorners = math.max(0, math.min(maxCorners, funnelState.leftFunnel.Length)); + var numCorners = funnelState.CalculateNextCornerIndices(maxCorners, cornerIndices, part.startPoint, part.endPoint, out bool lastCorner); + funnelState.ConvertCornerIndicesToPath(cornerIndices, numCorners, false, part.startPoint, part.endPoint, lastCorner, buffer); + + var nativeBufferSpan = buffer.AsUnsafeSpan(); + var endOfPart = (float3)part.endPoint; + return RemainingDistanceLowerBound(in nativeBufferSpan, in endOfPart, in movementPlane); + } + + [System.ThreadStatic] + private static List scratchList; + + /// + /// Calculate the next corners in the path. + /// + /// This will also do additional simplifications to the path if possible. Inner corners will be removed. + /// There is a limit to how many simplifications will be done per frame. + /// + /// If the path contains destroyed nodes, then will become true and a best-effort result will be returned. + /// + /// Note: This method may modify the PathTracer state, so it is not safe to call it from multiple threads at the same time. + /// + /// The buffer to store the corners in. The first corner will be the start point. + /// The maximum number of corners to store in the buffer. At least 2 corners will always be stored. + /// A temporary array to use for calculations. This array will be resized if it is too small or uninitialized. + /// The allocator to use for the scratchArray, if it needs to be reallocated. + /// The traversal provider to use for the path. Or null to use the default traversal provider. + /// The path to pass to the traversal provider. Or null. + public void GetNextCorners (NativeList buffer, int maxCorners, ref NativeArray scratchArray, Allocator allocator, ITraversalProvider traversalProvider, Path path) { + var numCorners = GetNextCornerIndices(ref scratchArray, maxCorners, allocator, out var lastCorner, traversalProvider, path); + var part = parts[firstPartIndex]; + funnelState.ConvertCornerIndicesToPath(scratchArray, numCorners, false, part.startPoint, part.endPoint, lastCorner, buffer); + } + + /// + /// Calculate the indices of the next corners in the path. + /// + /// This is like , except that it returns indices referring to the internal . + /// You can use or to convert the indices to world space positions. + /// + public int GetNextCornerIndices (ref NativeArray buffer, int maxCorners, Allocator allocator, out bool lastCorner, ITraversalProvider traversalProvider, Path path) { + const int MaxSimplifications = 3; + int allowedSimplifications = MaxSimplifications; + + // Treat start point as a corner + maxCorners -= 1; + if (scratchList == null) scratchList = new List(8); + var alternativePath = scratchList; + + while (true) { + // Limit max corners to the maximum possible number of corners given our current funnel. + // We have to do this in every iteration of the loop because the simplification may cause the funnel to contain more nodes/corners even if it is shorter. + var concreteMaxCorners = math.max(0, math.min(maxCorners, funnelState.leftFunnel.Length)); + if (!buffer.IsCreated || buffer.Length < concreteMaxCorners) { + if (buffer.IsCreated) buffer.Dispose(); + buffer = new NativeArray(math.ceilpow2(concreteMaxCorners), allocator, NativeArrayOptions.UninitializedMemory); + } + var cornerIndices = buffer; + + var part = parts[firstPartIndex]; + var numCorners = funnelState.CalculateNextCornerIndices(concreteMaxCorners, cornerIndices, part.startPoint, part.endPoint, out lastCorner); + + if (allowedSimplifications > 0) { + if (partGraphType == PartGraphType.Grid) { + MarkerSimplify.Begin(); + bool isInnerCorner = SimplifyGridInnerVertex(ref this.nodes, cornerIndices.AsUnsafeSpan().Slice(0, numCorners), part, ref portalIsNotInnerCorner, alternativePath, out int alternativeStartIndex, out int alternativeEndIndex, nnConstraint, traversalProvider, path, lastCorner); + MarkerSimplify.End(); + if (isInnerCorner) { + Profiler.BeginSample("Splice"); + if (SplicePath(alternativeStartIndex, alternativeEndIndex - alternativeStartIndex + 1, alternativePath)) { + allowedSimplifications -= 1; + version++; + Profiler.EndSample(); + continue; + } else { + firstPartContainsDestroyedNodes = true; + // Return best effort result + } + Profiler.EndSample(); + } + } else { + if (FirstInnerVertex(cornerIndices, numCorners, alternativePath, out int alternativeStartIndex, out int alternativeEndIndex, traversalProvider, path)) { + if (SplicePath(alternativeStartIndex, alternativeEndIndex - alternativeStartIndex + 1, alternativePath)) { + allowedSimplifications--; + version++; + continue; + } else { + firstPartContainsDestroyedNodes = true; + // Return best effort result + } + } + } + } + // We are either not allowed to simplify any more, or we couldn't find any simplification opportunities. Return the result. + return numCorners; + } + } + + /// + /// Converts corner indices to world space positions. + /// + /// The corners will not necessarily be in the same world space position as the real corners. Instead the path will be unwrapped and flattened, + /// and then transformed onto a plane that lines up with the first portal in the funnel. For most 2D and 3D worlds, this distinction is irrelevant, + /// but for curved worlds (e.g. a spherical world) this can make a big difference. In particular, steering towards unwrapped corners + /// is much more likely to work well than steering towards the real corners, as they can be e.g. on the other side of a round planet. + /// + /// The corner indices to convert. You can get these from #GetNextCornerIndices. + /// The number of indices in the cornerIndices array. + /// True if the last corner in the path has been reached. + /// The buffer to store the converted positions in. + /// The up axis of the agent's movement plane. + public void ConvertCornerIndicesToPathProjected (NativeArray cornerIndices, int numCorners, bool lastCorner, NativeList buffer, float3 up) { + var part = parts[firstPartIndex]; + funnelState.ConvertCornerIndicesToPathProjected(cornerIndices.AsUnsafeReadOnlySpan().Slice(0, numCorners), false, part.startPoint, part.endPoint, lastCorner, buffer, up); + } + + /// + /// Calculates a lower bound on the remaining distance to the end of the path part. + /// + /// It assumes the agent will follow the path, and then move in a straight line to the end of the path part. + /// + [BurstCompile] + public static float RemainingDistanceLowerBound (in UnsafeSpan nextCorners, in float3 endOfPart, in NativeMovementPlane movementPlane) { + if (nextCorners.Length == 0) return 0; + var prev = nextCorners[0]; + var remainingDistance = 0.0f; + for (int i = 1; i < nextCorners.Length; i++) { + var next = nextCorners[i]; + remainingDistance += math.length(movementPlane.ToPlane(next - prev)); + prev = next; + } + remainingDistance += math.length(movementPlane.ToPlane(prev - endOfPart)); + return remainingDistance; + } + + /// + /// Remove the first count parts of the path. + /// + /// This is used when an agent has traversed an off-mesh-link, and we want to start following the path after the off-mesh-link. + /// + /// The number of parts to remove. + /// The traversal provider to use for the path. Or null to use the default traversal provider. + /// The path to pass to the traversal provider. Or null. + public void PopParts (int count, ITraversalProvider traversalProvider, Path path) { + if (firstPartIndex + count >= parts.Length) throw new System.InvalidOperationException("Cannot pop the last part of a path"); + firstPartIndex += count; + version++; + var part = parts[firstPartIndex]; + while (nodes.AbsoluteStartIndex < part.startIndex) { + nodes.PopStart(); + nodeHashes.PopStart(); + } + this.startNode = nodes.Length > 0 ? nodes.First : null; + firstPartContainsDestroyedNodes = false; + if (GetPartType() == Funnel.PartType.OffMeshLink) { + this.partGraphType = PartGraphType.OffMeshLink; + SetFunnelState(part); + CheckInvariants(); + return; + } + this.partGraphType = PartGraphTypeFromNode(startNode); + + for (int i = part.startIndex; i <= part.endIndex; i++) { + if (!ValidInPath(i)) { + // The part contains invalid nodes. + // However, we didn't get a chance to calculate funnel portals for this part. + // In order to preserve the invariant that we have funnel portals for the first part, + // we must remove all nodes after and including this one. + // We mark the path as stale in order to trigger a path recalculation as soon as possible. + // If this is the first node in the part, we leave only this node, to ensure the path is not empty. + RemoveAllPartsExceptFirst(); + while (nodes.AbsoluteEndIndex > i) { + nodes.PopEnd(); + nodeHashes.PopEnd(); + } + part.endIndex = i; + parts[firstPartIndex] = part; + + if (i == part.startIndex) { + firstPartContainsDestroyedNodes = true; + // The part has no valid nodes. + // Replace it with a dummy part + Assert.IsTrue(nodes.Length == 1); + Assert.AreEqual(part.startIndex, part.endIndex); + // Invalid path, and we don't have any nodes to fall back on. + // This path tracer is completely invalid + funnelState.Clear(); + portalIsNotInnerCorner.Clear(); + startNode = null; + CheckInvariants(); + + // var nn = NNConstraint.Walkable; + // nn.distanceMetric = DistanceMetric.ClosestAsSeenFromAboveSoft(movementPlane.ToWorld(float2.zero, 1)); + // var globallyClosestNode = AstarPath.active != null ? AstarPath.active.GetNearest(unclampedStartPoint, nn) : NNInfo.Empty; + // if (globallyClosestNode.node != null) { + // SetFromSingleNode(globallyClosestNode.node, globallyClosestNode.position, movementPlane.value); + // } else { + + // } + return; + } else { + endIsUpToDate = false; + // It's not the first node, we can remove this node too + nodes.PopEnd(); + nodeHashes.PopEnd(); + part.endIndex = i - 1; + parts[firstPartIndex] = part; + break; + } + } + } + + if (partGraphType == PartGraphType.Grid) { + RemoveGridPathDiagonals(this.parts, firstPartIndex, ref this.nodes, ref this.nodeHashes, nnConstraint, traversalProvider, path); + part = parts[firstPartIndex]; + } + + SetFunnelState(part); + CheckInvariants(); + } + + void RemoveAllPartsExceptFirst () { + if (partCount <= 1) return; + var newParts = new Funnel.PathPart[1]; + newParts[0] = parts[firstPartIndex]; + this.parts = newParts; + firstPartIndex = 0; + // Remove all nodes in subsequent parts + while (nodes.AbsoluteEndIndex > parts[0].endIndex) { + nodes.PopEnd(); + nodeHashes.PopEnd(); + } + version++; + } + + /// Indicates if the given path part is a regular path part or an off-mesh link. + /// The index of the path part. Zero is the always the current path part. + public readonly Funnel.PartType GetPartType (int partIndex = 0) { + return parts[this.firstPartIndex + partIndex].type; + } + + public readonly bool PartContainsDestroyedNodes (int partIndex = 0) { + if (partIndex < 0 || partIndex >= partCount) throw new System.ArgumentOutOfRangeException(nameof(partIndex)); + + var part = parts[firstPartIndex + partIndex]; + for (int i = part.startIndex; i <= part.endIndex; i++) { + if (!ValidInPath(i)) return true; + } + return false; + } + + public OffMeshLinks.OffMeshLinkTracer GetLinkInfo (int partIndex = 0) { + if (partIndex < 0 || partIndex >= partCount) throw new System.ArgumentOutOfRangeException(nameof(partIndex)); + if (GetPartType(partIndex) != Funnel.PartType.OffMeshLink) throw new System.ArgumentException("Part is not an off-mesh link"); + var part = parts[firstPartIndex + partIndex]; + var startNode = nodes.GetAbsolute(part.startIndex) as LinkNode; + var endNode = nodes.GetAbsolute(part.endIndex) as LinkNode; + if (startNode == null) throw new System.Exception("Expected a link node"); + if (endNode == null) throw new System.Exception("Expected a link node"); + if (startNode.Destroyed) throw new System.Exception("Start node is destroyed"); + if (endNode.Destroyed) throw new System.Exception("End node is destroyed"); + + bool isReverse; + if (startNode.linkConcrete.startLinkNode == startNode) { + isReverse = false; + } else if (startNode.linkConcrete.startLinkNode == endNode) { + isReverse = true; + } else { + throw new System.Exception("Link node is not part of the link"); + } + return new OffMeshLinks.OffMeshLinkTracer(startNode.linkConcrete, isReverse); + } + + void SetFunnelState (Funnel.PathPart part) { + Profiler.BeginSample("SetFunnelState"); + this.funnelState.Clear(); + this.portalIsNotInnerCorner.Clear(); + + if (part.type == Funnel.PartType.NodeSequence) { + var startNode = nodes.GetAbsolute(part.startIndex); + if (startNode.Graph is GridGraph gridGraph) { + funnelState.projectionAxis = gridGraph.transform.WorldUpAtGraphPosition(Vector3.zero); + } + + var tmpLeft = ListPool.Claim(part.endIndex - part.startIndex); + var tmpRight = ListPool.Claim(part.endIndex - part.startIndex); + CalculateFunnelPortals(part.startIndex, part.endIndex, tmpLeft, tmpRight); + this.funnelState.Splice(0, 0, tmpLeft, tmpRight); + for (int i = 0; i < tmpLeft.Count; i++) { + this.portalIsNotInnerCorner.PushEnd(0); + } + ListPool.Release(ref tmpLeft); + ListPool.Release(ref tmpRight); + } + Profiler.EndSample(); + version++; + } + + void CalculateFunnelPortals (int startNodeIndex, int endNodeIndex, List outLeftPortals, List outRightPortals) { + Profiler.BeginSample("CalculatePortals"); + var prevNode = this.nodes.GetAbsolute(startNodeIndex); + AssertValidInPath(startNodeIndex); + + for (int i = startNodeIndex + 1; i <= endNodeIndex; i++) { + var node = this.nodes.GetAbsolute(i); + AssertValidInPath(i); + if (prevNode.GetPortal(node, out var left, out var right)) { + outLeftPortals.Add(left); + outRightPortals.Add(right); + } else { + throw new System.InvalidOperationException("Couldn't find a portal from " + prevNode + " " + node + " " + prevNode.ContainsOutgoingConnection(node)); + } + prevNode = node; + } + Profiler.EndSample(); + } + + /// Replaces the current path with a single node + public void SetFromSingleNode (GraphNode node, Vector3 position, NativeMovementPlane movementPlane) { + SetPath( + new List { + new Funnel.PathPart { startIndex = 0, endIndex = 0, startPoint = position, endPoint = position } + }, + new List { node }, + position, + position, + movementPlane, + null, + null + ); + } + + /// Clears the current path + public void Clear () { + funnelState.Clear(); + parts = null; + nodes.Clear(); + nodeHashes.Clear(); + portalIsNotInnerCorner.Clear(); + unclampedEndPoint = unclampedStartPoint = Vector3.zero; + firstPartIndex = 0; + startIsUpToDate = false; + endIsUpToDate = false; + firstPartContainsDestroyedNodes = false; + startNodeInternal = null; + partGraphType = PartGraphType.Navmesh; + CheckInvariants(); + } + + static int2 ResolveNormalizedGridPoint (GridGraph grid, ref CircularBuffer nodes, UnsafeSpan cornerIndices, Funnel.PathPart part, int index, out int nodeIndex) { + if (index < 0 || index >= cornerIndices.Length) { + var p = index < 0 ? part.startPoint : part.endPoint; + nodeIndex = index < 0 ? part.startIndex : part.endIndex; + var pointInGraphSpace = grid.transform.InverseTransform(p); + var node = nodes.GetAbsolute(nodeIndex) as GridNodeBase; + var nodeCoords = node.CoordinatesInGrid; + var normalizedPoint = new int2( + math.clamp((int)(GridGraph.FixedPrecisionScale * (pointInGraphSpace.x - nodeCoords.x)), 0, GridGraph.FixedPrecisionScale), + math.clamp((int)(GridGraph.FixedPrecisionScale * (pointInGraphSpace.z - nodeCoords.y)), 0, GridGraph.FixedPrecisionScale) + ); + return normalizedPoint; + } else { + var rightSide = (cornerIndices[index] & Funnel.RightSideBit) != 0; + nodeIndex = part.startIndex + (cornerIndices[index] & Funnel.FunnelPortalIndexMask); + Assert.IsTrue(nodeIndex >= part.startIndex && nodeIndex < part.endIndex); + + var node = nodes.GetAbsolute(nodeIndex) as GridNodeBase; + var node2 = nodes.GetAbsolute(nodeIndex + 1) as GridNodeBase; + var node1Coords = node.CoordinatesInGrid; + var node2Coords = node2.CoordinatesInGrid; + var dx = node2Coords.x - node1Coords.x; + var dz = node2Coords.y - node1Coords.y; + var dir = GridNodeBase.OffsetToConnectionDirection(dx, dz); + if (dir > 4) { + throw new System.Exception("Diagonal connections are not supported"); + } + + // Both of these values will be either -1 or +1. + var vertexDx = GridGraph.neighbourXOffsets[dir] + GridGraph.neighbourXOffsets[(dir + (rightSide ? -1 : 1) + 4) % 4]; + var vertexDz = GridGraph.neighbourZOffsets[dir] + GridGraph.neighbourZOffsets[(dir + (rightSide ? -1 : 1) + 4) % 4]; + return new int2(512 + 512*vertexDx, 512 + 512*vertexDz); + } + } + + static int[] SplittingCoefficients = new int[] { + 0, 1, + 1, 2, + 1, 4, + 3, 4, + 1, 8, + 3, 8, + 5, 8, + 7, 8, + }; + + private static readonly ProfilerMarker MarkerSimplify = new ProfilerMarker("Simplify"); + + static bool SimplifyGridInnerVertex (ref CircularBuffer nodes, UnsafeSpan cornerIndices, Funnel.PathPart part, ref CircularBuffer portalIsNotInnerCorner, List alternativePath, out int alternativeStartIndex, out int alternativeEndIndex, NNConstraint nnConstraint, ITraversalProvider traversalProvider, Path path, bool lastCorner) { + var corners = lastCorner ? cornerIndices.Length : cornerIndices.Length - 1; + alternativeStartIndex = -1; + alternativeEndIndex = -1; + if (corners == 0) { + return false; + } + + // Only try to simplify every 2^value frame, unless the path changes + const int EveryNthLog2 = 2; + int i = 0; + var idx = cornerIndices[i]; + var rightSide = (idx & Funnel.RightSideBit) != 0; + var portalIndex = idx & Funnel.FunnelPortalIndexMask; + var splitting = portalIsNotInnerCorner[portalIndex] % (8 * (1 << EveryNthLog2)); + portalIsNotInnerCorner[portalIndex] = (byte)(splitting + 1); + + // Only try to simplify every 2^n'th frame + if ((splitting & ((1 << EveryNthLog2) - 1)) != 0) { + return false; + } + splitting /= 1 << EveryNthLog2; + + Assert.IsTrue(portalIndex >= 0 && portalIndex < part.endIndex - part.startIndex); + + // The ResolveNormalizedGridPoint method will access up to the cornerIndices[1] node and the one that follows it in the path + var lastRelevantNodeIndex = cornerIndices.length < 2 ? part.endIndex : math.min(part.endIndex, part.startIndex + (cornerIndices[1] & Funnel.FunnelPortalIndexMask) + 1); + for (int j = part.startIndex; j < lastRelevantNodeIndex; j++) { + var a = nodes.GetAbsolute(j); + var b = nodes.GetAbsolute(j + 1); + if (!Valid(b) || !a.ContainsOutgoingConnection(b)) { + // The path is no longer valid + return false; + } + } + + var grid = GridNode.GetGridGraph(nodes.GetAbsolute(part.startIndex).GraphIndex); + + var normalizedPointStart = ResolveNormalizedGridPoint(grid, ref nodes, cornerIndices, part, i - 1, out var startNodeIndex); + var normalizedPointEnd = ResolveNormalizedGridPoint(grid, ref nodes, cornerIndices, part, i + 1, out var endNodeIndex); + var normalizedPointMid = ResolveNormalizedGridPoint(grid, ref nodes, cornerIndices, part, i, out var midNodeIndex); + + var startNode = nodes.GetAbsolute(startNodeIndex) as GridNodeBase; + var midNode = nodes.GetAbsolute(midNodeIndex) as GridNodeBase; + var endNode = nodes.GetAbsolute(endNodeIndex) as GridNodeBase; + + // Try to simplify using different linecast target points every frame. + // Assume we have the path + // + // A ----- B + // \ + // \ + // C + // + // Where A is the start point, B is a corner of the funnel, and C is either the next corner or the end point of the path. + // Then we can pick any point on the line between B and C as the linecast target point. The origin will always be A. + if (splitting > 0) { + var a = SplittingCoefficients[splitting*2]; + var b = SplittingCoefficients[splitting*2+1]; + endNodeIndex = endNodeIndex + (midNodeIndex - endNodeIndex)*a/b; + if (endNodeIndex == midNodeIndex) return false; + + var endCoords = endNode.CoordinatesInGrid; + var midCoords = midNode.CoordinatesInGrid; + var mp = new int2(midCoords.x * 1024, midCoords.y * 1024) + normalizedPointMid; + var ep = new int2(endCoords.x * 1024, endCoords.y * 1024) + normalizedPointEnd; + endNode = nodes.GetAbsolute(endNodeIndex) as GridNodeBase; + endCoords = endNode.CoordinatesInGrid; + var f = VectorMath.ClosestPointOnLineFactor(new Int2(mp.x, mp.y), new Int2(ep.x, ep.y), new Int2(endCoords.x * 1024 + 512, endCoords.y * 1024 + 512)); + var p = new int2((int)math.lerp(mp.x, ep.x, f), (int)math.lerp(mp.y, ep.y, f)) - new int2(endCoords.x * 1024, endCoords.y * 1024); + normalizedPointEnd = new int2( + math.clamp(p.x, 0, GridGraph.FixedPrecisionScale), + math.clamp(p.y, 0, GridGraph.FixedPrecisionScale) + ); + } + + alternativePath.Clear(); + var obstructed = grid.Linecast(startNode, normalizedPointStart, endNode, normalizedPointEnd, out var _, alternativePath, null, false); + if (!obstructed) { + Assert.AreEqual(startNode, alternativePath[0]); + Assert.AreEqual(endNode, alternativePath[alternativePath.Count-1]); + + // The linecast was unobstructed. But the new path may still be more costly than the old path, if any penalties are used. + // The traversal provider or the NNConstraint may also disallow the new path. + for (int j = 1; j < alternativePath.Count; j++) { + if (traversalProvider != null ? !traversalProvider.CanTraverse(path, alternativePath[j-1], alternativePath[j]) : !nnConstraint.Suitable(alternativePath[j])) { + return false; + } + } + uint alternativeCost = 0; + for (int j = 0; j < alternativePath.Count; j++) { + alternativeCost += traversalProvider != null? traversalProvider.GetTraversalCost(path, alternativePath[j]) : DefaultITraversalProvider.GetTraversalCost(path, alternativePath[j]); + } + + if (alternativeCost > 0) { + // The new path *may* be more costly than the old one. + // We have to do a thorough check to see if the new path is better. + + // Calculate the cost of the old path. + uint oldCost = 0; + for (int j = startNodeIndex; j <= endNodeIndex; j++) { + oldCost += traversalProvider != null? traversalProvider.GetTraversalCost(path, nodes.GetAbsolute(j)) : DefaultITraversalProvider.GetTraversalCost(path, nodes.GetAbsolute(j)); + } + + if (alternativeCost > oldCost) { + return false; + } + } + alternativeStartIndex = startNodeIndex; + alternativeEndIndex = endNodeIndex; + return true; + } else { + return false; + } + } + + + /// + /// Removes diagonal connections in a grid path and replaces them with two axis-aligned connections. + /// + /// This is done to make the funnel algorithm work better on grid graphs. + /// + static void RemoveGridPathDiagonals (Funnel.PathPart[] parts, int partIndex, ref CircularBuffer path, ref CircularBuffer pathNodeHashes, NNConstraint nnConstraint, ITraversalProvider traversalProvider, Path pathObject) { + int inserted = 0; + var part = parts != null ? parts[partIndex] : new Funnel.PathPart { startIndex = path.AbsoluteStartIndex, endIndex = path.AbsoluteEndIndex }; + for (int i = part.endIndex - 1; i >= part.startIndex; i--) { + var node = path.GetAbsolute(i) as GridNodeBase; + var node2 = path.GetAbsolute(i + 1) as GridNodeBase; + var dx = node2.XCoordinateInGrid - node.XCoordinateInGrid; + var dz = node2.ZCoordinateInGrid - node.ZCoordinateInGrid; + var dir = GridNodeBase.OffsetToConnectionDirection(dx, dz); + if (dir >= 4) { + var d1 = dir - 4; + var d2 = (dir - 4 + 1) % 4; + var n1 = node.GetNeighbourAlongDirection(d1); + if (n1 != null && (traversalProvider != null ? !traversalProvider.CanTraverse(pathObject, node, n1) : !nnConstraint.Suitable(n1))) n1 = null; + + if (n1 != null && n1.GetNeighbourAlongDirection(d2) == node2 && (traversalProvider == null || traversalProvider.CanTraverse(pathObject, n1, node2))) { + path.InsertAbsolute(i+1, n1); + if (pathNodeHashes.Length > 0) pathNodeHashes.InsertAbsolute(i+1, HashNode(n1)); + inserted++; + } else { + var n2 = node.GetNeighbourAlongDirection(d2); + if (n2 != null && (traversalProvider != null ? !traversalProvider.CanTraverse(pathObject, node, n2) : !nnConstraint.Suitable(n2))) n2 = null; + + if (n2 != null && n2.GetNeighbourAlongDirection(d1) == node2 && (traversalProvider == null || traversalProvider.CanTraverse(pathObject, n2, node2))) { + path.InsertAbsolute(i+1, n2); + if (pathNodeHashes.Length > 0) pathNodeHashes.InsertAbsolute(i+1, HashNode(n2)); + inserted++; + } else { + throw new System.Exception("Axis-aligned connection not found"); + } + } + } + } + + if (parts != null) { + parts[partIndex].endIndex += inserted; + for (int i = partIndex + 1; i < parts.Length; i++) { + parts[i].startIndex += inserted; + parts[i].endIndex += inserted; + } + } + } + + static PartGraphType PartGraphTypeFromNode (GraphNode node) { + if (node == null) { + return PartGraphType.Navmesh; + } else if (node is GridNodeBase) { + return PartGraphType.Grid; + } else if (node is TriangleMeshNode) { + return PartGraphType.Navmesh; + } else { + throw new System.Exception("The PathTracer (and by extension FollowerEntity component) cannot be used on graphs of type " + node.Graph.GetType().Name); + } + } + + /// Replaces the current path with the given path. + /// The path to follow. + /// The movement plane of the agent. + public void SetPath (ABPath path, NativeMovementPlane movementPlane) { + var parts = Funnel.SplitIntoParts(path); + // Copy settings from the path's NNConstraint to the path tracer + nnConstraint.constrainTags = path.nnConstraint.constrainTags; + nnConstraint.tags = path.nnConstraint.tags; + nnConstraint.graphMask = path.nnConstraint.graphMask; + nnConstraint.constrainWalkability = path.nnConstraint.constrainWalkability; + nnConstraint.walkable = path.nnConstraint.walkable; + + SetPath(parts, path.path, path.originalStartPoint, path.originalEndPoint, movementPlane, path.traversalProvider, path); + Pathfinding.Util.ListPool.Release(ref parts); + } + + /// Replaces the current path with the given path. + /// The individual parts of the path. See \reflink{Funnel.SplitIntoParts}. + /// All nodes in the path. The path parts refer to slices of this array. + /// The start point of the path. This is typically the start point that was passed to the path request, or the agent's current position. + /// The end point of the path. This is typically the destination point that was passed to the path request. + /// The movement plane of the agent. + /// The traversal provider to use for the path. Or null to use the default traversal provider. + /// The path to pass to the traversal provider. Or null. + public void SetPath (List parts, List nodes, Vector3 unclampedStartPoint, Vector3 unclampedEndPoint, NativeMovementPlane movementPlane, ITraversalProvider traversalProvider, Path path) { + this.startNode = nodes.Count > 0 ? nodes[0] : null; + partGraphType = PartGraphTypeFromNode(this.startNode); + this.unclampedEndPoint = unclampedEndPoint; + this.unclampedStartPoint = unclampedStartPoint; + firstPartContainsDestroyedNodes = false; + startIsUpToDate = true; + endIsUpToDate = true; + this.parts = parts.ToArray(); + this.nodes.Clear(); + this.nodes.AddRange(nodes); + this.nodeHashes.Clear(); + for (int i = 0; i < nodes.Count; i++) { + this.nodeHashes.PushEnd(HashNode(nodes[i])); + } + this.firstPartIndex = 0; + + if (partGraphType == PartGraphType.Grid) { + // SimplifyGridPath(this.parts, 0, ref this.nodes, int.MaxValue); + RemoveGridPathDiagonals(this.parts, 0, ref this.nodes, ref this.nodeHashes, nnConstraint, traversalProvider, path); + } + + SetFunnelState(this.parts[firstPartIndex]); + version++; + CheckInvariants(); + // This is necessary because the path may not have used the same distance metric that the path tracer uses. + // And if we don't do this, then the start/end points of the funnel may be slightly incorrect. + Repair(unclampedStartPoint, true, RepairQuality.Low, movementPlane, traversalProvider, path, false); + Repair(unclampedEndPoint, false, RepairQuality.Low, movementPlane, traversalProvider, path, false); + } + + /// Returns a deep clone of this object + public PathTracer Clone () { + return new PathTracer { + parts = parts != null? parts.Clone() as Funnel.PathPart[] : null, + nodes = nodes.Clone(), + portalIsNotInnerCorner = portalIsNotInnerCorner.Clone(), + funnelState = funnelState.Clone(), + unclampedEndPoint = unclampedEndPoint, + unclampedStartPoint = unclampedStartPoint, + startNodeInternal = startNodeInternal, + + firstPartIndex = firstPartIndex, + startIsUpToDate = startIsUpToDate, + endIsUpToDate = endIsUpToDate, + firstPartContainsDestroyedNodes = firstPartContainsDestroyedNodes, + version = version, + nnConstraint = NNConstraint.Walkable, + partGraphType = partGraphType, + }; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathTracer.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathTracer.cs.meta new file mode 100644 index 0000000..ac4c008 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathTracer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2ecd3295b97b21b4491934afc7c8a60b +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs new file mode 100644 index 0000000..9a8a4cc --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs @@ -0,0 +1,747 @@ +using Pathfinding.Util; +using System.Collections.Generic; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine; +using System.Linq; + +namespace Pathfinding { + /// + /// Contains useful functions for working with paths and nodes. + /// This class works a lot with the class, a useful function to get nodes is AstarPath.GetNearest. + /// See: + /// See: + /// See: + /// + public static class PathUtilities { + /// + /// Returns if there is a walkable path from node1 to node2. + /// This method is extremely fast because it only uses precalculated information. + /// + /// + /// GraphNode node1 = AstarPath.active.GetNearest(point1, NNConstraint.Walkable).node; + /// GraphNode node2 = AstarPath.active.GetNearest(point2, NNConstraint.Walkable).node; + /// + /// if (PathUtilities.IsPathPossible(node1, node2)) { + /// // Yay, there is a path between those two nodes + /// } + /// + /// + /// Equivalent to calling with a list containing node1 and node2. + /// + /// See: graph-updates (view in online documentation for working links) + /// See: + /// See: + /// + public static bool IsPathPossible (GraphNode node1, GraphNode node2) { + return node1.Walkable && node2.Walkable && node1.Area == node2.Area; + } + + /// + /// Returns if there are walkable paths between all nodes in the list. + /// + /// Returns true for empty lists. + /// + /// See: graph-updates (view in online documentation for working links) + /// See: + /// + public static bool IsPathPossible (List nodes) { + if (nodes.Count == 0) return true; + + uint area = nodes[0].Area; + for (int i = 0; i < nodes.Count; i++) if (!nodes[i].Walkable || nodes[i].Area != area) return false; + return true; + } + + /// + /// Returns if there are walkable paths between all nodes in the list. + /// + /// This method will actually only check if the first node can reach all other nodes. However this is + /// equivalent in 99% of the cases since almost always the graph connections are bidirectional. + /// If you are not aware of any cases where you explicitly create unidirectional connections + /// this method can be used without worries. + /// + /// Returns true for empty lists + /// + /// Warning: This method is significantly slower than the IsPathPossible method which does not take a tagMask + /// + /// See: graph-updates (view in online documentation for working links) + /// See: + /// + public static bool IsPathPossible (List nodes, int tagMask) { + if (nodes.Count == 0) return true; + + // Make sure that the first node has a valid tag + if (((tagMask >> (int)nodes[0].Tag) & 1) == 0) return false; + + // Fast check first + if (!IsPathPossible(nodes)) return false; + + // Make sure that the first node can reach all other nodes + var reachable = GetReachableNodes(nodes[0], tagMask); + bool result = true; + + // Make sure that the first node can reach all other nodes + for (int i = 1; i < nodes.Count; i++) { + if (!reachable.Contains(nodes[i])) { + result = false; + break; + } + } + + // Pool the temporary list + ListPool.Release(ref reachable); + + return result; + } + + /// + /// Returns all nodes reachable from the seed node. + /// This function performs a DFS (depth-first-search) or flood fill of the graph and returns all nodes which can be reached from + /// the seed node. In almost all cases this will be identical to returning all nodes which have the same area as the seed node. + /// In the editor areas are displayed as different colors of the nodes. + /// The only case where it will not be so is when there is a one way path from some part of the area to the seed node + /// but no path from the seed node to that part of the graph. + /// + /// The returned list is not sorted in any particular way. + /// + /// Depending on the number of reachable nodes, this function can take quite some time to calculate + /// so don't use it too often or it might affect the framerate of your game. + /// + /// See: bitmasks (view in online documentation for working links). + /// + /// Returns: A List containing all nodes reachable from the seed node. + /// For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool. + /// + /// The node to start the search from. + /// Optional mask for tags. This is a bitmask. + /// Optional filter for which nodes to search. You can combine this with tagMask = -1 to make the filter determine everything. + /// Only walkable nodes are searched regardless of the filter. If the filter function returns false the node will be treated as unwalkable. + public static List GetReachableNodes (GraphNode seed, int tagMask = -1, System.Func filter = null) { + Stack dfsStack = StackPool.Claim(); + List reachable = ListPool.Claim(); + + /// TODO: Pool + var map = new HashSet(); + + System.Action callback; + // Check if we can use the fast path + if (tagMask == -1 && filter == null) { + callback = (GraphNode node) => { + if (node.Walkable && map.Add(node)) { + reachable.Add(node); + dfsStack.Push(node); + } + }; + } else { + callback = (GraphNode node) => { + if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && map.Add(node)) { + if (filter != null && !filter(node)) return; + + reachable.Add(node); + dfsStack.Push(node); + } + }; + } + + callback(seed); + + while (dfsStack.Count > 0) { + dfsStack.Pop().GetConnections(callback); + } + + StackPool.Release(dfsStack); + return reachable; + } + + static Queue BFSQueue; + static Dictionary BFSMap; + + /// + /// Returns all nodes up to a given node-distance from the seed node. + /// This function performs a BFS (breadth-first search) or flood fill of the graph and returns all nodes within a specified node distance which can be reached from + /// the seed node. In almost all cases when depth is large enough this will be identical to returning all nodes which have the same area as the seed node. + /// In the editor areas are displayed as different colors of the nodes. + /// The only case where it will not be so is when there is a one way path from some part of the area to the seed node + /// but no path from the seed node to that part of the graph. + /// + /// The returned list is sorted by node distance from the seed node + /// i.e distance is measured in the number of nodes the shortest path from seed to that node would pass through. + /// Note that the distance measurement does not take heuristics, penalties or tag penalties. + /// + /// Depending on the number of nodes, this function can take quite some time to calculate + /// so don't use it too often or it might affect the framerate of your game. + /// + /// Returns: A List containing all nodes reachable up to a specified node distance from the seed node. + /// For better memory management the returned list should be pooled, see Pathfinding.Util.ListPool + /// + /// Warning: This method is not thread safe. Only use it from the Unity thread (i.e normal game code). + /// + /// The video below shows the BFS result with varying values of depth. Points are sampled on the nodes using . + /// [Open online documentation to see videos] + /// + /// + /// var seed = AstarPath.active.GetNearest(transform.position, NNConstraint.Walkable).node; + /// var nodes = PathUtilities.BFS(seed, 10); + /// foreach (var node in nodes) { + /// Debug.DrawRay((Vector3)node.position, Vector3.up, Color.red, 10); + /// } + /// + /// + /// The node to start the search from. + /// The maximum node-distance from the seed node. + /// Optional mask for tags. This is a bitmask. + /// Optional filter for which nodes to search. You can combine this with depth = int.MaxValue and tagMask = -1 to make the filter determine everything. + /// Only walkable nodes are searched regardless of the filter. If the filter function returns false the node will be treated as unwalkable. + public static List BFS (GraphNode seed, int depth, int tagMask = -1, System.Func filter = null) { + BFSQueue = BFSQueue ?? new Queue(); + var que = BFSQueue; + + BFSMap = BFSMap ?? new Dictionary(); + var map = BFSMap; + + // Even though we clear at the end of this function, it is good to + // do it here as well in case the previous invocation of the method + // threw an exception for some reason + // and didn't clear the que and map + que.Clear(); + map.Clear(); + + List result = ListPool.Claim(); + + int currentDist = -1; + System.Action callback; + if (tagMask == -1) { + callback = node => { + if (node.Walkable && !map.ContainsKey(node)) { + if (filter != null && !filter(node)) return; + + map.Add(node, currentDist+1); + result.Add(node); + que.Enqueue(node); + } + }; + } else { + callback = node => { + if (node.Walkable && ((tagMask >> (int)node.Tag) & 0x1) != 0 && !map.ContainsKey(node)) { + if (filter != null && !filter(node)) return; + + map.Add(node, currentDist+1); + result.Add(node); + que.Enqueue(node); + } + }; + } + + callback(seed); + + while (que.Count > 0) { + GraphNode n = que.Dequeue(); + currentDist = map[n]; + + if (currentDist >= depth) break; + + n.GetConnections(callback); + } + + que.Clear(); + map.Clear(); + return result; + } + + /// + /// Returns points in a spiral centered around the origin with a minimum clearance from other points. + /// The points are laid out on the involute of a circle + /// See: http://en.wikipedia.org/wiki/Involute + /// Which has some nice properties. + /// All points are separated by clearance world units. + /// This method is O(n), yes if you read the code you will see a binary search, but that binary search + /// has an upper bound on the number of steps, so it does not yield a log factor. + /// + /// Note: Consider recycling the list after usage to reduce allocations. + /// See: Pathfinding.Util.ListPool + /// + public static List GetSpiralPoints (int count, float clearance) { + List pts = ListPool.Claim(count); + + // The radius of the smaller circle used for generating the involute of a circle + // Calculated from the separation distance between the turns + float a = clearance/(2*Mathf.PI); + float t = 0; + + + pts.Add(InvoluteOfCircle(a, t)); + + for (int i = 0; i < count; i++) { + Vector3 prev = pts[pts.Count-1]; + + // d = -t0/2 + sqrt( t0^2/4 + 2d/a ) + // Minimum angle (radians) which would create an arc distance greater than clearance + float d = -t/2 + Mathf.Sqrt(t*t/4 + 2*clearance/a); + + // Binary search for separating this point and the previous one + float mn = t + d; + float mx = t + 2*d; + while (mx - mn > 0.01f) { + float mid = (mn + mx)/2; + Vector3 p = InvoluteOfCircle(a, mid); + if ((p - prev).sqrMagnitude < clearance*clearance) { + mn = mid; + } else { + mx = mid; + } + } + + pts.Add(InvoluteOfCircle(a, mx)); + t = mx; + } + + return pts; + } + + /// + /// Returns the XZ coordinate of the involute of circle. + /// See: http://en.wikipedia.org/wiki/Involute + /// + private static Vector3 InvoluteOfCircle (float a, float t) { + return new Vector3(a*(Mathf.Cos(t) + t*Mathf.Sin(t)), 0, a*(Mathf.Sin(t) - t*Mathf.Cos(t))); + } + + /// + /// Will calculate a number of points around p which are on the graph and are separated by clearance from each other. + /// This is like GetPointsAroundPoint except that previousPoints are treated as being in world space. + /// The average of the points will be found and then that will be treated as the group center. + /// + /// The point to generate points around + /// The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph. + /// Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best. + /// The points to use for reference. Note that these are in world space. + /// The new points will overwrite the existing points in the list. The result will be in world space. + /// The final points will be at most this distance from p. + /// The points will if possible be at least this distance from each other. + public static void GetPointsAroundPointWorld (Vector3 p, IRaycastableGraph g, List previousPoints, float radius, float clearanceRadius) { + if (previousPoints.Count == 0) return; + + Vector3 avg = Vector3.zero; + for (int i = 0; i < previousPoints.Count; i++) avg += previousPoints[i]; + avg /= previousPoints.Count; + + for (int i = 0; i < previousPoints.Count; i++) previousPoints[i] -= avg; + + GetPointsAroundPoint(p, g, previousPoints, radius, clearanceRadius); + } + + /// + /// Will calculate a number of points around center which are on the graph and are separated by clearance from each other. + /// The maximum distance from center to any point will be radius. + /// Points will first be tried to be laid out as previousPoints and if that fails, random points will be selected. + /// This is great if you want to pick a number of target points for group movement. If you pass all current agent points from e.g the group's average position + /// this method will return target points so that the units move very little within the group, this is often aesthetically pleasing and reduces jitter if using + /// some kind of local avoidance. + /// + /// TODO: Write unit tests + /// + /// The point to generate points around + /// The graph to use for linecasting. If you are only using one graph, you can get this by AstarPath.active.graphs[0] as IRaycastableGraph. + /// Note that not all graphs are raycastable, recast, navmesh and grid graphs are raycastable. On recast and navmesh it works the best. + /// The points to use for reference. Note that these should not be in world space. They are treated as relative to center. + /// The new points will overwrite the existing points in the list. The result will be in world space, not relative to center. + /// The final points will be at most this distance from center. + /// The points will if possible be at least this distance from each other. + public static void GetPointsAroundPoint (Vector3 center, IRaycastableGraph g, List previousPoints, float radius, float clearanceRadius) { + if (g == null) throw new System.ArgumentNullException("g"); + + var graph = g as NavGraph; + + if (graph == null) throw new System.ArgumentException("g is not a NavGraph"); + + var nn = graph.GetNearest(center, NNConstraint.Walkable); + center = nn.position; + + if (nn.node == null) { + // No valid point to start from + return; + } + + + // Make sure the enclosing circle has a radius which can pack circles with packing density 0.5 + radius = Mathf.Max(radius, 1.4142f*clearanceRadius*Mathf.Sqrt(previousPoints.Count)); //Mathf.Sqrt(previousPoints.Count*clearanceRadius*2)); + clearanceRadius *= clearanceRadius; + + for (int i = 0; i < previousPoints.Count; i++) { + Vector3 dir = previousPoints[i]; + float magn = dir.magnitude; + + if (magn > 0) dir /= magn; + + float newMagn = radius;//magn > radius ? radius : magn; + dir *= newMagn; + + GraphHitInfo hit; + + int tests = 0; + while (true) { + Vector3 pt = center + dir; + + if (g.Linecast(center, pt, out hit)) { + if (hit.point == Vector3.zero) { + // Oops, linecast actually failed completely + // try again unless we have tried lots of times + // then we just continue anyway + tests++; + if (tests > 8) { + previousPoints[i] = pt; + break; + } + } else { + pt = hit.point; + } + } + + bool worked = false; + + for (float q = 0.1f; q <= 1.0f; q += 0.05f) { + Vector3 qt = Vector3.Lerp(center, pt, q); + worked = true; + for (int j = 0; j < i; j++) { + if ((previousPoints[j] - qt).sqrMagnitude < clearanceRadius) { + worked = false; + break; + } + } + + // Abort after 8 tests or when we have found a valid point + if (worked || tests > 8) { + worked = true; + previousPoints[i] = qt; + break; + } + } + + // Break out of nested loop + if (worked) { + break; + } + + // If we could not find a valid point, reduce the clearance radius slightly to improve + // the chances next time + clearanceRadius *= 0.9f; + // This will pick points in 2D closer to the edge of the circle with a higher probability + dir = UnityEngine.Random.onUnitSphere * Mathf.Lerp(newMagn, radius, tests / 5); + dir.y = 0; + tests++; + } + } + } + + [BurstCompile(FloatMode = FloatMode.Fast)] + struct JobFormationPacked : IJob { + public NativeArray positions; + public float3 destination; + public float agentRadius; + public NativeMovementPlane movementPlane; + + public float CollisionTime (float2 pos1, float2 pos2, float2 v1, float2 v2, float r1, float r2) { + var relativeVelocity = v1 - v2; + + if (math.all(relativeVelocity == float2.zero)) { + // No collision + return float.MaxValue; + } + var radius = r1 + r2; + var relativePos = pos2 - pos1; + var relativeDir = math.normalize(relativeVelocity); + var d1 = math.dot(relativePos, relativeDir); + var d2sq = math.lengthsq(relativePos - relativeDir * d1); + var offsetSq = radius*radius - d2sq; + if (offsetSq <= 0) { + // No collision + return float.MaxValue; + } + var offset = math.sqrt(offsetSq); + var collisionDistance = d1 - offset; + if (collisionDistance < -radius) { + // No collision (collision is in the imagined past) + return float.MaxValue; + } + return collisionDistance * math.rsqrt(math.lengthsq(relativeVelocity)); + //return collisionDistance / math.length(relativeVelocity); + } + + struct DistanceComparer : IComparer { + public NativeArray positions; + + public int Compare (int x, int y) { + return (int)math.sign(math.lengthsq(positions[x]) - math.lengthsq(positions[y])); + } + } + + public void Execute () { + if (positions.Length == 0) return; + + NativeArray positions2D = new NativeArray(positions.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + NativeArray indices = new NativeArray(positions.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < positions.Length; i++) { + positions2D[i] = movementPlane.ToPlane(positions[i]); + indices[i] = i; + } + + float2 mean = float2.zero; + for (int i = 0; i < positions2D.Length; i++) { + mean += positions2D[i]; + } + mean /= positions2D.Length; + + for (int i = 0; i < positions2D.Length; i++) { + positions2D[i] -= mean; + } + + // Sort agents by their distance to the center + indices.Sort(new DistanceComparer { positions = positions2D }); + + NativeArray minTimes = new NativeArray(positions.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int a = 0; a < positions.Length; a++) { + var ta = float.MaxValue; + var ia = indices[a]; + for (int b = 0; b < a; b++) { + var ib = indices[b]; + //float tb = CollisionTime(positions2D[ia], positions2D[ib], -positions2D[ia], -positions2D[ib], agentRadius, agentRadius); + float tb = CollisionTime(positions2D[ia], positions2D[ib], -positions2D[ia], float2.zero, agentRadius, agentRadius); + ta = math.min(ta, tb); + } + minTimes[ia] = ta; + positions2D[ia] -= positions2D[ia] * math.min(1.0f, minTimes[indices[a]]); + } + + for (int i = 0; i < positions.Length; i++) { + positions[i] = movementPlane.ToWorld(positions2D[i]) + destination; + } + } + } + + public static void FormationPacked (List currentPositions, Vector3 destination, float clearanceRadius, NativeMovementPlane movementPlane) { + var positions = new NativeArray(currentPositions.Count, Allocator.TempJob, NativeArrayOptions.UninitializedMemory); + + for (int i = 0; i < positions.Length; i++) positions[i] = currentPositions[i]; + new JobFormationPacked { + positions = positions, + destination = destination, + agentRadius = clearanceRadius, + movementPlane = movementPlane, + }.Schedule().Complete(); + for (int i = 0; i < positions.Length; i++) currentPositions[i] = positions[i]; + positions.Dispose(); + } + + public enum FormationMode { + SinglePoint, + Packed, + } + + public static List FormationDestinations (List group, Vector3 destination, FormationMode formationMode, float marginFactor = 0.1f) { + if (group.Count == 0) return new List(); + + var positions = group.Select(u => u.position).ToList(); + + if (formationMode == FormationMode.SinglePoint) { + for (int i = 0; i < positions.Count; i++) positions[i] = destination; + } else { + var previousMean = Vector3.zero; + for (int i = 0; i < positions.Count; i++) previousMean += positions[i]; + previousMean /= positions.Count; + + // Assume the whole group uses the same movement plane, or at least a similar one + var movementPlane = group[0].movementPlane; + Debug.Log(((Quaternion)movementPlane.rotation).eulerAngles); + + // Figure out if the group if the destination point is in the middle of the group, + // or if it is outside the group + var standardDeviation = Mathf.Sqrt(positions.Average(p => Vector3.SqrMagnitude(p - previousMean))); + var thresholdDistance = standardDeviation*1.0f; + + if (Vector3.Distance(destination, previousMean) > thresholdDistance) { + // If the destination is outside of the group, use a packed formation + Pathfinding.PathUtilities.FormationPacked(positions, destination, group[0].radius * (1 + marginFactor), movementPlane); + } else { + // If the destination is inside the group, move all agents to the same point + for (int i = 0; i < positions.Count; i++) positions[i] = destination; + } + } + + return positions; + } + + class ConstrainToSet : NNConstraint { + public HashSet nodes; + + public override bool Suitable (GraphNode node) { + return nodes.Contains(node); + } + } + + public static void GetPointsAroundPointWorldFlexible (Vector3 center, Quaternion rotation, List positions) { + if (positions.Count == 0) return; + + var snapped = AstarPath.active.GetNearest(center, NNConstraint.Walkable); + + // Move slightly toward the node center just to avoid the group center being on a node edge + var groupPos = Vector3.Lerp(snapped.position, (Vector3)snapped.node.position, 0.001f); + + var previousMean = Vector3.zero; + for (int i = 0; i < positions.Count; i++) previousMean += positions[i]; + previousMean /= positions.Count; + + var maxSqrDistance = 0f; + for (int i = 0; i < positions.Count; i++) { + positions[i] -= previousMean; + maxSqrDistance = Mathf.Max(maxSqrDistance, positions[i].sqrMagnitude); + } + + // Multiplying by 4 doubles the normal distance + maxSqrDistance *= 2*2; + + // Search at least this number of nodes regardless of the distance to the nodes + int minNodes = 10; + + var nodes = PathUtilities.BFS(snapped.node, int.MaxValue, -1, node => { + minNodes--; + return minNodes > 0 || ((Vector3)node.position - groupPos).sqrMagnitude < maxSqrDistance; + }); + + NNConstraint nn = new ConstrainToSet() { + nodes = new HashSet(nodes) + }; + + int iterations = 3; + for (int k = 0; k < iterations; k++) { + float totalWeight = 0f; + Vector3 totalSum = Vector3.zero; + + for (int i = 0; i < positions.Count; i++) { + var rel = rotation * positions[i]; + var p = groupPos + rel; + + var near = AstarPath.active.GetNearest(p, nn).position; + // TODO: Handle case when no close node was found + + var weight = Vector3.Distance(p, near); + + totalSum += (near - rel) * weight; + totalWeight += weight; + } + + // If no changes were required, then break early + if (totalWeight <= 0.0000001f) break; + + var newCenter = totalSum / totalWeight; + + groupPos = AstarPath.active.GetNearest(newCenter, nn).position; + } + + for (int i = 0; i < positions.Count; i++) { + positions[i] = groupPos + rotation * positions[i]; + } + } + + /// + /// Returns randomly selected points on the specified nodes with each point being separated by clearanceRadius from each other. + /// Selecting points ON the nodes only works for TriangleMeshNode (used by Recast Graph and Navmesh Graph) and GridNode (used by GridGraph). + /// For other node types, only the positions of the nodes will be used. + /// + /// clearanceRadius will be reduced if no valid points can be found. + /// + /// Note: This method assumes that the nodes in the list have the same type for some special cases. + /// More specifically if the first node is not a TriangleMeshNode or a GridNode, it will use a fast path + /// which assumes that all nodes in the list have the same surface area (which usually is a surface area of zero and the + /// nodes are all PointNodes). + /// + public static List GetPointsOnNodes (List nodes, int count, float clearanceRadius = 0) { + if (nodes == null) throw new System.ArgumentNullException("nodes"); + if (nodes.Count == 0) throw new System.ArgumentException("no nodes passed"); + + List pts = ListPool.Claim(count); + + // Square + clearanceRadius *= clearanceRadius; + + if (clearanceRadius > 0 || nodes[0] is TriangleMeshNode +#if !ASTAR_NO_GRID_GRAPH + || nodes[0] is GridNode +#endif + ) { + // Accumulated area of all nodes + List accs = ListPool.Claim(nodes.Count); + + // Total area of all nodes so far + float tot = 0; + + for (int i = 0; i < nodes.Count; i++) { + var surfaceArea = nodes[i].SurfaceArea(); + // Ensures that even if the nodes have a surface area of 0, a random one will still be picked + // instead of e.g always picking the first or the last one. + surfaceArea += 0.001f; + tot += surfaceArea; + accs.Add(tot); + } + + for (int i = 0; i < count; i++) { + // Pick point + int testCount = 0; + int testLimit = 10; + + while (true) { + bool worked = true; + + // If no valid points could be found, progressively lower the clearance radius until such a point is found + if (testCount >= testLimit) { + // Note that clearanceRadius is a squared radius + clearanceRadius *= 0.9f*0.9f; + testLimit += 10; + if (testLimit > 100) clearanceRadius = 0; + } + + // Pick a random node among the ones in the list weighted by their area + float tg = UnityEngine.Random.value*tot; + int v = accs.BinarySearch(tg); + if (v < 0) v = ~v; + + if (v >= nodes.Count) { + // Cover edge cases + continue; + } + + var node = nodes[v]; + var p = node.RandomPointOnSurface(); + + // Test if it is some distance away from the other points + if (clearanceRadius > 0) { + for (int j = 0; j < pts.Count; j++) { + if ((pts[j]-p).sqrMagnitude < clearanceRadius) { + worked = false; + break; + } + } + } + + if (worked) { + pts.Add(p); + break; + } + testCount++; + } + } + + ListPool.Release(ref accs); + } else { + // Fast path, assumes all nodes have the same area (usually zero) + for (int i = 0; i < count; i++) { + pts.Add((Vector3)nodes[UnityEngine.Random.Range(0, nodes.Count)].RandomPointOnSurface()); + } + } + + return pts; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs.meta new file mode 100644 index 0000000..7803e88 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/PathUtilities.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 7d93ae7d64ab84e23819ac5754065f34 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/ProceduralGraphMover.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/ProceduralGraphMover.cs new file mode 100644 index 0000000..8bfeaa0 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/ProceduralGraphMover.cs @@ -0,0 +1,257 @@ +using UnityEngine; +using System.Collections; + +namespace Pathfinding { + using Pathfinding.Util; + using Unity.Mathematics; + using UnityEngine.Profiling; + using Pathfinding.Graphs.Navmesh; + using Pathfinding.Jobs; + using Pathfinding.Drawing; + using System.Collections.Generic; + using Unity.Jobs; + + /// + /// Moves a grid or recast graph to follow a target. + /// + /// This is useful if you have a very large, or even infinite, world, but pathfinding is only necessary in a small region around an object (for example the player). + /// This component will move a graph around so that its center stays close to the object. + /// + /// Note: This component can only be used with grid graphs, layered grid graphs and (tiled) recast graphs. + /// + /// Usage + /// Take a look at the example scene called "Procedural" for an example of how to use this script + /// + /// Attach this to some object in the scene and assign the target to e.g the player. + /// Then the graph will follow that object around as it moves. + /// + /// [Open online documentation to see videos] + /// + /// [Open online documentation to see videos] + /// + /// Performance + /// When the graph is moved you may notice an fps drop. + /// If this grows too large you can try a few things: + /// + /// General advice: + /// - Turn on multithreading (A* Inspector -> Settings) + /// - Make sure you have 'Show Graphs' disabled in the A* inspector, since gizmos in the scene view can take some + /// time to update when the graph moves, and thus make it seem like this script is slower than it actually is. + /// + /// For grid graphs: + /// - Avoid using any erosion in the grid graph settings. This is relatively slow. Each erosion iteration requires expanding the region that is updated by 1 node. + /// - Reduce the grid size or resolution. + /// - Reduce the . This will make the updates smaller but more frequent. + /// This only works to some degree however since an update has an inherent overhead. + /// - Disable Height Testing or Collision Testing in the grid graph if you can. This can give a performance boost + /// since fewer calls to the physics engine need to be done. + /// + /// For recast graphs: + /// - Rasterize colliders instead of meshes. This is typically faster. + /// - Use a reasonable tile size. Very small tiles can cause more overhead, and too large tiles might mean that you are updating too much in one go. + /// Typical values are around 64 to 256 voxels. + /// - Use a larger cell size. A lower cell size will give better quality graphs, but it will also be slower to scan. + /// + /// The graph updates will be offloaded to worker threads as much as possible. + /// + /// See: large-worlds (view in online documentation for working links) + /// + [AddComponentMenu("Pathfinding/Procedural Graph Mover")] + [HelpURL("https://arongranberg.com/astar/documentation/stable/proceduralgraphmover.html")] + public class ProceduralGraphMover : VersionedMonoBehaviour { + /// + /// Grid graphs will be updated if the target is more than this number of nodes from the graph center. + /// Note that this is in nodes, not world units. + /// + /// Note: For recast graphs, this setting has no effect. + /// + public float updateDistance = 10; + + /// Graph will be moved to follow this target + public Transform target; + + /// True while the graph is being updated by this script + public bool updatingGraph { get; private set; } + + /// + /// Graph to update. + /// This will be set at Start based on . + /// During runtime you may set this to any graph or to null to disable updates. + /// + public NavGraph graph; + + /// + /// Index for the graph to update. + /// This will be used at Start to set . + /// + /// This is an index into the AstarPath.active.data.graphs array. + /// + [HideInInspector] + public int graphIndex; + + void Start () { + if (AstarPath.active == null) throw new System.Exception("There is no AstarPath object in the scene"); + + // If one creates this component via a script then they may have already set the graph field. + // In that case don't replace it. + if (graph == null) { + if (graphIndex < 0) throw new System.Exception("Graph index should not be negative"); + if (graphIndex >= AstarPath.active.data.graphs.Length) throw new System.Exception("The ProceduralGraphMover was configured to use graph index " + graphIndex + ", but only " + AstarPath.active.data.graphs.Length + " graphs exist"); + + graph = AstarPath.active.data.graphs[graphIndex]; + if (!(graph is GridGraph || graph is RecastGraph)) throw new System.Exception("The ProceduralGraphMover was configured to use graph index " + graphIndex + " but that graph either does not exist or is not a GridGraph, LayerGridGraph or RecastGraph"); + + if (graph is RecastGraph rg && !rg.useTiles) Debug.LogWarning("The ProceduralGraphMover component only works with tiled recast graphs. Enable tiling in the recast graph inspector.", this); + } + + UpdateGraph(); + } + + void OnDisable () { + // Just in case this script is performing an update while being disabled + if (AstarPath.active != null) AstarPath.active.FlushWorkItems(); + + updatingGraph = false; + } + + /// Update is called once per frame + void Update () { + if (AstarPath.active == null || graph == null || !graph.isScanned) return; + + if (graph is GridGraph gg) { + // Calculate where the graph center and the target position is in graph space + // In graph space, (0,0,0) is bottom left corner of the graph + // For grid graphs, one unit along the X and Z axes in graph space equals the distance between two nodes. + // The Y axis still uses world units + var graphCenterInGraphSpace = gg.transform.InverseTransform(gg.center); + var targetPositionInGraphSpace = gg.transform.InverseTransform(target.position); + + // Check the distance in graph space + // We only care about the X and Z axes since the Y axis is the "height" coordinate of the nodes (in graph space) + // We only care about the plane that the nodes are placed in + if (VectorMath.SqrDistanceXZ(graphCenterInGraphSpace, targetPositionInGraphSpace) > updateDistance*updateDistance) { + UpdateGraph(); + } + } else if (graph is RecastGraph rg) { + UpdateGraph(); + } else { + throw new System.Exception("ProceduralGraphMover cannot be used with graphs of type " + graph.GetType().Name); + } + } + + /// + /// Updates the graph asynchronously. + /// This will move the graph so that the target's position is (roughly) the center of the graph. + /// If the graph is already being updated, the call will be ignored. + /// + /// The image below shows which nodes will be updated when the graph moves. + /// The whole graph is not recalculated each time it is moved, but only those + /// nodes that have to be updated, the rest will keep their old values. + /// The image is a bit simplified but it shows the main idea. + /// [Open online documentation to see images] + /// + /// If you want to move the graph synchronously then pass false to the async parameter. + /// + public void UpdateGraph (bool async = true) { + if (!enabled) throw new System.InvalidOperationException("This component has been disabled"); + + if (updatingGraph) { + // We are already updating the graph + // so ignore this call + return; + } + + if (graph is GridGraph gg) { + UpdateGridGraph(gg, async); + } else if (graph is RecastGraph rg) { + var delta = RecastGraphTileShift(rg, target.position); + if (delta.x != 0 || delta.y != 0) { + updatingGraph = true; + UpdateRecastGraph(rg, delta, async); + } + } + } + + void UpdateGridGraph (GridGraph graph, bool async) { + // Start a work item for updating the graph + // This will pause the pathfinding threads + // so that it is safe to update the graph + // and then do it over several frames + // to avoid too large FPS drops + + updatingGraph = true; + List<(IGraphUpdatePromise, IEnumerator)> promises = new List<(IGraphUpdatePromise, IEnumerator)>(); + AstarPath.active.AddWorkItem(new AstarWorkItem( + ctx => { + // Find the direction that we want to move the graph in. + // Calculate this in graph space (where a distance of one is the size of one node) + Vector3 dir = graph.transform.InverseTransformVector(target.position - graph.center); + + // Snap to a whole number of nodes to offset in each direction + int dx = Mathf.RoundToInt(dir.x); + int dz = Mathf.RoundToInt(dir.z); + + if (dx != 0 || dz != 0) { + var promise = graph.TranslateInDirection(dx, dz); + promises.Add((promise, promise.Prepare())); + } + }, + (ctx, force) => { + if (GraphUpdateProcessor.ProcessGraphUpdatePromises(promises, ctx, force)) { + updatingGraph = false; + return true; + } + return false; + } + )); + if (!async) AstarPath.active.FlushWorkItems(); + } + + static Int2 RecastGraphTileShift (RecastGraph graph, Vector3 targetCenter) { + // Find the direction that we want to move the graph in. + // Calcuculate this in graph space, to take the graph rotation into account + Vector3 dir = graph.transform.InverseTransform(targetCenter) - graph.transform.InverseTransform(graph.forcedBoundsCenter); + + // Only move in one direction at a time for simplicity + if (Mathf.Abs(dir.x) > Mathf.Abs(dir.z)) dir.z = 0; + else dir.x = 0; + + // Calculate how many whole tiles to move. + // Avoid moving unless we want to move at least 0.5+#Hysteresis full tiles + // Hysteresis must be at least 0. + const float Hysteresis = 0.2f; + return new Int2( + (int)(Mathf.Max(0, Mathf.Abs(dir.x) / graph.TileWorldSizeX + 0.5f - Hysteresis) * Mathf.Sign(dir.x)), + (int)(Mathf.Max(0, Mathf.Abs(dir.z) / graph.TileWorldSizeZ + 0.5f - Hysteresis) * Mathf.Sign(dir.z)) + ); + } + + void UpdateRecastGraph (RecastGraph graph, Int2 delta, bool async) { + updatingGraph = true; + List<(IGraphUpdatePromise, IEnumerator)> promises = new List<(IGraphUpdatePromise, IEnumerator)>(); + AstarPath.active.AddWorkItem(new AstarWorkItem( + ctx => { + var promise = graph.TranslateInDirection(delta.x, delta.y); + promises.Add((promise, promise.Prepare())); + }, + (ctx, force) => { + if (GraphUpdateProcessor.ProcessGraphUpdatePromises(promises, ctx, force)) { + updatingGraph = false; + return true; + } + return false; + } + )); + if (!async) AstarPath.active.FlushWorkItems(); + } + } + + /// + /// This class has been renamed to . + /// + /// Deprecated: Use instead + /// + [System.Obsolete("This class has been renamed to ProceduralGraphMover", true)] + public class ProceduralGridMover { + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/ProceduralGraphMover.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/ProceduralGraphMover.cs.meta new file mode 100644 index 0000000..6e90edc --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/ProceduralGraphMover.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a634230476dc478b88eceac73b1c8a4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {fileID: 2800000, guid: ddf758bc3c379c843a3c9b6030f06959, type: 3} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Promise.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Promise.cs new file mode 100644 index 0000000..9cfc0e4 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Promise.cs @@ -0,0 +1,48 @@ +using Unity.Jobs; + +namespace Pathfinding.Util { + public interface IProgress { + float Progress { get; } + } + + /// + /// A promise that T is being calculated asynchronously. + /// + /// The result can be accessed by calling . This will block until the calculation is complete. + /// + public struct Promise: IProgress, System.IDisposable where T : IProgress, System.IDisposable { + public JobHandle handle; + T result; + + public Promise(JobHandle handle, T result) { + this.handle = handle; + this.result = result; + } + + public bool IsCompleted { + get { + return handle.IsCompleted; + } + } + + public float Progress { + get { + return result.Progress; + } + } + + public T GetValue () { + return result; + } + + public T Complete () { + handle.Complete(); + return result; + } + + public void Dispose () { + Complete(); + result.Dispose(); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Promise.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Promise.cs.meta new file mode 100644 index 0000000..e955030 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/Promise.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ffac5a6557b05460bb1adc051e20f4ca +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/RWLock.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/RWLock.cs new file mode 100644 index 0000000..0cdd360 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/RWLock.cs @@ -0,0 +1,215 @@ +// #define DEBUG_RWLOCK +using Unity.Jobs; + +namespace Pathfinding.Jobs { + /// + /// A simple read/write lock for use with the Unity Job System. + /// + /// The RW-lock makes the following assumptions: + /// - Only the main thread will call the methods on this lock. + /// - If jobs are to use locked data, you should call or on the lock and pass the returned JobHandle as a dependency the job, and then call on the lock object, with the newly scheduled job's handle. + /// - When taking a Read lock, you should only read data, but if you take a Write lock you may modify data. + /// - On the main thread, multiple synchronous write locks may be nested. + /// + /// You do not need to care about dependencies when calling the and methods. That's handled automatically for you. + /// + /// See: https://en.wikipedia.org/wiki/Readers%E2%80%93writer_lock + /// + /// + /// var readLock = AstarPath.active.LockGraphDataForReading(); + /// var handle = new MyJob { + /// // ... + /// }.Schedule(readLock.dependency); + /// readLock.UnlockAfter(handle); + /// + /// + public class RWLock { + JobHandle lastWrite; + JobHandle lastRead; +#if ENABLE_UNITY_COLLECTIONS_CHECKS + int heldSyncLocks; + bool pendingAsync; +#if DEBUG_RWLOCK + string pendingStackTrace; +#endif + + void CheckPendingAsync () { +#if DEBUG_RWLOCK + if (pendingAsync) throw new System.InvalidOperationException("An async lock was previously aquired, but UnlockAfter was never called on it. The lock was aquired at\n" + pendingStackTrace + "\n\n"); +#else + if (pendingAsync) throw new System.InvalidOperationException("An async lock was previously aquired, but UnlockAfter was never called on it."); +#endif + } +#endif + + void AddPendingSync () { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + CheckPendingAsync(); +#if DEBUG_RWLOCK + pendingStackTrace = System.Environment.StackTrace; +#endif + heldSyncLocks++; +#endif + } + + void RemovePendingSync () { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (heldSyncLocks <= 0) throw new System.InvalidOperationException("Tried to unlock a lock which was not locked. Did you call Unlock twice?"); + heldSyncLocks--; +#endif + } + + void AddPendingAsync () { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + CheckPendingAsync(); +#if DEBUG_RWLOCK + if (heldSyncWriteLocks > 0) throw new System.InvalidOperationException("A synchronous lock is already being held. You cannot lock it asynchronously at the same time. The sync lock was aquired at\n" + pendingStackTrace + "\n\n"); + pendingStackTrace = System.Environment.StackTrace; +#else + if (heldSyncLocks > 0) throw new System.InvalidOperationException("A synchronous lock is already being held. You cannot lock it asynchronously at the same time."); +#endif + pendingAsync = true; +#endif + } + + void RemovePendingAsync () { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + pendingAsync = false; +#endif + } + + /// + /// Aquire a read lock on the main thread. + /// This method will block until all pending write locks have been released. + /// + public LockSync ReadSync () { + AddPendingSync(); + lastWrite.Complete(); + lastWrite = default; // Setting this to default will avoid a call into unity's c++ parts next time we call Complete (improves perf slightly) + return new LockSync(this); + } + + /// + /// Aquire a read lock on the main thread. + /// This method will not block until all asynchronous write locks have been released, instead you should make sure to add the returned JobHandle as a dependency to any jobs that use the locked data. + /// + /// If a synchronous write lock is currently held, this method will throw an exception. + /// + /// + /// var readLock = AstarPath.active.LockGraphDataForReading(); + /// var handle = new MyJob { + /// // ... + /// }.Schedule(readLock.dependency); + /// readLock.UnlockAfter(handle); + /// + /// + public ReadLockAsync Read () { + AddPendingAsync(); + return new ReadLockAsync(this, lastWrite); + } + + /// + /// Aquire a write lock on the main thread. + /// This method will block until all pending read and write locks have been released. + /// + public LockSync WriteSync () { + AddPendingSync(); + lastWrite.Complete(); + lastWrite = default; // Setting this to default will avoid a call into unity's c++ parts next time we call Complete (improves perf slightly) + lastRead.Complete(); + return new LockSync(this); + } + + /// + /// Aquire a write lock on the main thread. + /// This method will not block until all asynchronous read and write locks have been released, instead you should make sure to add the returned JobHandle as a dependency to any jobs that use the locked data. + /// + /// If a synchronous write lock is currently held, this method will throw an exception. + /// + /// + /// var readLock = AstarPath.active.LockGraphDataForReading(); + /// var handle = new MyJob { + /// // ... + /// }.Schedule(readLock.dependency); + /// readLock.UnlockAfter(handle); + /// + /// + public WriteLockAsync Write () { + AddPendingAsync(); + return new WriteLockAsync(this, JobHandle.CombineDependencies(lastRead, lastWrite)); + } + + public readonly struct CombinedReadLockAsync { + readonly RWLock lock1; + readonly RWLock lock2; + public readonly JobHandle dependency; + + public CombinedReadLockAsync(ReadLockAsync lock1, ReadLockAsync lock2) { + this.lock1 = lock1.inner; + this.lock2 = lock2.inner; + dependency = JobHandle.CombineDependencies(lock1.dependency, lock2.dependency); + } + + /// Release the lock after the given job has completed + public void UnlockAfter (JobHandle handle) { + if (lock1 != null) { + lock1.RemovePendingAsync(); + lock1.lastRead = JobHandle.CombineDependencies(lock1.lastRead, handle); + } + if (lock2 != null) { + lock2.RemovePendingAsync(); + lock2.lastRead = JobHandle.CombineDependencies(lock2.lastRead, handle); + } + } + } + + public readonly struct ReadLockAsync { + internal readonly RWLock inner; + public readonly JobHandle dependency; + + public ReadLockAsync(RWLock inner, JobHandle dependency) { + this.inner = inner; + this.dependency = dependency; + } + + /// Release the lock after the given job has completed + public void UnlockAfter (JobHandle handle) { + if (inner != null) { + inner.RemovePendingAsync(); + inner.lastRead = JobHandle.CombineDependencies(inner.lastRead, handle); + } + } + } + + public readonly struct WriteLockAsync { + readonly RWLock inner; + public readonly JobHandle dependency; + + public WriteLockAsync(RWLock inner, JobHandle dependency) { + this.inner = inner; + this.dependency = dependency; + } + + /// Release the lock after the given job has completed + public void UnlockAfter (JobHandle handle) { + if (inner != null) { + inner.RemovePendingAsync(); + inner.lastWrite = handle; + } + } + } + + public readonly struct LockSync { + readonly RWLock inner; + + public LockSync(RWLock inner) { + this.inner = inner; + } + + /// Release the lock + public void Unlock () { + if (inner != null) inner.RemovePendingSync(); + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/RWLock.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/RWLock.cs.meta new file mode 100644 index 0000000..dfe7fcf --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/RWLock.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 78c62f7a9ebf8614db5f43ee33f118b0 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/SpinLock.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/SpinLock.cs new file mode 100644 index 0000000..88b9092 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/SpinLock.cs @@ -0,0 +1,28 @@ +using System.Threading; + +namespace Pathfinding.Jobs { + /// + /// Spin lock which can be used in Burst. + /// Good when the lock is generally uncontested. + /// Very inefficient when the lock is contested. + /// + public struct SpinLock { + private volatile int locked; + + public void Lock () { + while (Interlocked.CompareExchange(ref locked, 1, 0) != 0) + Unity.Burst.Intrinsics.Common.Pause(); // spin + + // We need to ensure that any optimizer does not reorder loads to before we aquire the lock. + System.Threading.Thread.MemoryBarrier(); + } + + public void Unlock () { + // We need a memory barrier to ensure that all writes are visible to other threads, before we unlock. + // We also need to ensure that any optimizer does not reorder stores to after the unlock. + System.Threading.Thread.MemoryBarrier(); + // Release the lock by writing 0 to it. Use atomics to make it immediately visible to other threads. + if (Interlocked.Exchange(ref locked, 0) == 0) throw new System.InvalidOperationException("Trying to unlock a lock which is not locked"); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/SpinLock.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/SpinLock.cs.meta new file mode 100644 index 0000000..d5889d3 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/SpinLock.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2a281c042e9d12449b73bda8e3828059 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/UnityReferenceHelper.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/UnityReferenceHelper.cs new file mode 100644 index 0000000..e4d895c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/UnityReferenceHelper.cs @@ -0,0 +1,39 @@ +using Pathfinding.Util; +using UnityEngine; + +namespace Pathfinding { + [ExecuteInEditMode] + /// + /// Helper class to keep track of references to GameObjects. + /// Does nothing more than to hold a GUID value. + /// + [HelpURL("https://arongranberg.com/astar/documentation/stable/unityreferencehelper.html")] + public class UnityReferenceHelper : MonoBehaviour { + [HideInInspector] + [SerializeField] + private string guid; + + public string GetGUID() => guid; + + public void Awake () { + Reset(); + } + + public void Reset () { + if (string.IsNullOrEmpty(guid)) { + guid = Pathfinding.Util.Guid.NewGuid().ToString(); + Debug.Log("Created new GUID - " + guid, this); + } else if (gameObject.scene.name != null) { + // Create a new GUID if there are duplicates in the scene. + // Don't do this if this is a prefab (scene.name == null) + foreach (UnityReferenceHelper urh in UnityCompatibility.FindObjectsByTypeUnsorted()) { + if (urh != this && guid == urh.guid) { + guid = Pathfinding.Util.Guid.NewGuid().ToString(); + Debug.Log("Created new GUID - " + guid, this); + return; + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/UnityReferenceHelper.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/UnityReferenceHelper.cs.meta new file mode 100644 index 0000000..27a8921 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/UnityReferenceHelper.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 632ae7eb1d4ce49d38a1e38ee7efe7c5 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} -- cgit v1.1-26-g67d0