chore: initial commit

Assets/PathBerserker2d/Scripts/PathBerserker2d/AssemblyInfo.cs

@@ -0,0 +1,11 @@
+using System.Reflection;
+using System.Runtime.CompilerServices;
+
+[assembly: InternalsVisibleTo("PathBerserker2d.Editor")]
+[assembly: InternalsVisibleTo("PathBerserker2d.Upgrade")]
+[assembly: AssemblyVersion("2.1")]
+
+internal static class AssemblyInfo
+{
+    public static string Version => typeof(AssemblyInfo).Assembly.GetName().Version.ToString();
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/AssemblyInfo.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 8fed312cd3761ca4f9b284e6f05e37e8
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/BoostClipper.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: dbd7032f0753f4943b51d2103bd60b60
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/BoostClipper/ClipperWrapper.cs

@@ -0,0 +1,136 @@
+using ClipperLib;
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    class ClipperWrapper : IClipper
+    {
+        const int FloatToIntMult = 10000;
+        const float IntToFloatDiv = 10000;
+
+        Clipper clipper = new Clipper();
+
+        public ResultType Compute(Polygon sp, Polygon cp, BoolOpType op, out List<Polygon> result, bool includeOpenPolygons = false)
+        {
+            result = new List<Polygon>();
+
+            if (!sp.BoundsOverlap(cp))
+            {
+                return ResultType.NoOverlap;
+            }
+
+            AddPolygonToClipper(sp, PolyType.ptSubject);
+            AddPolygonToClipper(cp, PolyType.ptClip);
+
+            double prevArea = 0;
+            ClipType clipType;
+            switch (op)
+            {
+                case BoolOpType.INTERSECTION:
+                    clipType = ClipType.ctIntersection;
+                    break;
+                case BoolOpType.UNION:
+                    clipType = ClipType.ctUnion;
+                    break;
+                case BoolOpType.DIFFERENCE:
+                    clipType = ClipType.ctDifference;
+                    prevArea = sp.SignedArea();
+                    break;
+                default:
+                    throw new ArgumentException("Unknown op type " + op);
+            }
+
+            PolyTree resultTree = new PolyTree();
+            bool succeeded = clipper.Execute(clipType, resultTree);
+
+            GetResultsFromNode(resultTree, result, includeOpenPolygons);
+            foreach (var poly in result)
+            {
+                poly.EnsureCWOrdering();
+            }
+
+            clipper.Clear();
+
+            bool intersectionHappened = false;
+            double afterArea = 0;
+            switch (op)
+            {
+                case BoolOpType.INTERSECTION:
+                    intersectionHappened = result.Count > 0;
+                    break;
+                case BoolOpType.UNION:
+                    if (result.Count == 1)
+                        intersectionHappened = true;
+                    break;
+                case BoolOpType.DIFFERENCE:
+                    if (result.Count > 1)
+                        intersectionHappened = true;
+                    else
+                    {
+                        foreach (var poly in result)
+                            afterArea += poly.SignedArea();
+
+                        intersectionHappened = !(Math.Abs(afterArea - prevArea) < 0.001);
+                    }
+                    break;
+                default:
+                    throw new ArgumentException("Unknown op type " + op);
+            }
+
+
+            return intersectionHappened ? ResultType.Clipped : ResultType.NoOverlap;
+        }
+
+        private void AddPolygonToClipper(Polygon polygon, PolyType polyType)
+        {
+            var points = ConvertContour(polygon.Hull);
+            clipper.AddPath(points, polyType, polygon.Hull.IsClosed);
+
+            foreach (var hole in polygon.Holes)
+            {
+                points = ConvertContour(hole);
+                clipper.AddPath(points, polyType, hole.IsClosed);
+            }
+        }
+
+        private List<IntPoint> ConvertContour(Contour contour)
+        {
+            List<IntPoint> points = new List<IntPoint>(contour.VertexCount);
+            for (int i = 0; i < contour.VertexCount; i++)
+            {
+                points.Add(new IntPoint(contour.Verts[i].x * FloatToIntMult, contour.Verts[i].y * FloatToIntMult));
+            }
+            return points;
+        }
+
+        private void GetResultsFromNode(PolyNode node, List<Polygon> polygons, bool includeOpenPolygons)
+        {
+            foreach (var child in node.Childs)
+            {
+                if (child.IsOpen && !includeOpenPolygons)
+                    continue;
+
+                Polygon p = new Polygon(ConvertChain(child.m_polygon, !child.IsOpen));
+                polygons.Add(p);
+                foreach (var holeNode in child.Childs)
+                {
+                    var hole = ConvertChain(holeNode.m_polygon, !holeNode.IsOpen);
+                    p.Holes.Add(hole);
+
+                    GetResultsFromNode(holeNode, polygons, includeOpenPolygons);
+                }
+            }
+        }
+
+        private Contour ConvertChain(List<IntPoint> chain, bool closed)
+        {
+            return new Contour(chain.Select(ip => new Vector2(ip.X / IntToFloatDiv, ip.Y / IntToFloatDiv)), closed);
+        }
+
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/BoostClipper/ClipperWrapper.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 9276a8b2beda8b54ba7afae4535ba503
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/BoostClipper/License.txt

@@ -0,0 +1,24 @@
+Boost Software License - Version 1.0 - August 17th, 2003
+http://www.boost.org/LICENSE_1_0.txt
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.

Assets/PathBerserker2d/Scripts/PathBerserker2d/BoostClipper/License.txt.meta

@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: bf69683b55c9a474ab9f1585d2c62c61
+TextScriptImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/BoostClipper/clipper.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: e13902fa43e71754f9e04b2f44d5f71b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/Clipping.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: be8365f59449a0a4aa11aa4ad834006f
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/Clipping/Contour.cs

@@ -0,0 +1,123 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    internal class Contour : IEnumerable<Vector2>
+    {
+        public int VertexCount { get { return verts.Count; } }
+        public bool IsEmpty { get { return verts.Count == 0; } }
+        public bool IsClosed { get; private set; }
+        public List<Vector2> Verts => verts;
+
+        private List<Vector2> verts;
+
+        public Contour(List<Vector2> verticies, bool isClosed = true)
+        {
+            this.verts = verticies;
+            IsClosed = isClosed;
+        }
+
+        public Contour(IEnumerable<Vector2> verticies, bool isClosed = true)
+        {
+            this.verts = new List<Vector2>(verticies);
+            IsClosed = isClosed;
+        }
+
+        public Vector2 this[int key]
+        {
+            get { return verts[key]; }
+        }
+
+        public IEnumerator<Vector2> GetEnumerator()
+        {
+            return ((IEnumerable<Vector2>)verts).GetEnumerator();
+        }
+
+        IEnumerator IEnumerable.GetEnumerator()
+        {
+            return ((IEnumerable<Vector2>)verts).GetEnumerator();
+        }
+
+        public double SignedArea()
+        {
+            double area = 0;
+            for (int i = 0, j = verts.Count - 1; i < verts.Count; j = i, i++)
+                area += (verts[i].x - verts[j].x) *
+                        (verts[i].y + verts[j].y);
+            return area / 2.0;
+        }
+
+        public double Area()
+        {
+            return Math.Abs(SignedArea());
+        }
+
+        public void MakeCW()
+        {
+            if (!IsCW())
+                verts.Reverse();
+        }
+
+        public void Invert()
+        {
+            verts.Reverse();
+        }
+
+        public bool IsCW()
+        {
+            Vector2 lowestPoint = verts[0];
+            int lowestPointIndex = 0;
+            for (int i = 1; i < VertexCount; i++)
+            {
+                if (lowestPoint.y > verts[i].y ||
+                    lowestPoint.y == verts[i].y && lowestPoint.x < verts[i].x)
+                {
+                    lowestPoint = verts[i];
+                    lowestPointIndex = i;
+                }
+            }
+
+            Vector2 prevPoint = lowestPointIndex == 0 ? verts[VertexCount - 1] : verts[lowestPointIndex - 1];
+            Vector2 nextPoint = lowestPointIndex == VertexCount - 1 ? verts[0] : verts[lowestPointIndex + 1];
+
+            return ((lowestPoint.x - prevPoint.x) * (nextPoint.y - prevPoint.y) - (nextPoint.x - prevPoint.x) * (lowestPoint.y - prevPoint.y)) < 0;
+        }
+
+        public bool PointInContour(Vector2 point)
+        {
+            bool c = false;
+            for (int i = 0, j = VertexCount - 1; i < VertexCount; j = i++)
+            {
+                if (((verts[i].y > point.y) != (verts[j].y > point.y)) &&
+                 (point.x < (verts[j].x - verts[i].x) * (point.y - verts[i].y) / (verts[j].y - verts[i].y) + verts[i].x))
+                    c = !c;
+            }
+            return c;
+        }
+
+        public bool Contains(Contour other)
+        {
+            foreach (var v in other)
+                if (!PointInContour(v))
+                    return false;
+            return true;
+        }
+
+        public void Draw()
+        {
+            for (int i = IsClosed ? 0 : 1, j = IsClosed ? VertexCount - 1 : 0; i < VertexCount; j = i, i++)
+            {
+                DebugDrawingExtensions.DrawArrow(verts[i], verts[j]);
+            }
+        }
+
+        public void Simplify(float tolerance)
+        {
+            ExtendedGeometry.MergeCloseVerts(verts, tolerance);
+            this.verts = ExtendedGeometry.SimplifyContour(this.verts, tolerance);
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/Clipping/Contour.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: dad3920a5830b8e438bc7c5d3c92636d
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/Clipping/IClipper.cs

@@ -0,0 +1,16 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+using System.Threading.Tasks;
+
+namespace PathBerserker2d
+{
+    public enum ResultType { NoOverlap, Clipped };
+    public enum BoolOpType { INTERSECTION, UNION, DIFFERENCE };
+
+    interface IClipper
+    {
+        ResultType Compute(Polygon sp, Polygon cp, BoolOpType op, out List<Polygon> result, bool includeOpenPolygons = false);
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/Clipping/IClipper.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: e6f0c0aa6263ff54ca7f6be8694b9fa7
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/Clipping/Polygon.cs

@@ -0,0 +1,162 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    internal class Polygon : System.IEquatable<Polygon>, IEnumerable<Contour>
+    {
+        public Contour Hull => hull;
+        public List<Contour> Holes { get; set; }
+        public bool IsEmpty => hull.IsEmpty;
+        public float XMax => boundingRect.xMax;
+        public Rect BoundingRect => boundingRect;
+
+        private Contour hull;
+        private Rect boundingRect;
+
+        public Polygon(Contour hull, List<Contour> holes)
+        {
+            this.hull = hull;
+            this.Holes = holes;
+
+            UpdateBounds();
+        }
+
+        public Polygon(Contour hull)
+        {
+            this.hull = hull;
+            this.Holes = new List<Contour>(0);
+
+            UpdateBounds();
+        }
+
+        public Polygon()
+        {
+            this.hull = new Contour(new Vector2[0]);
+            this.Holes = new List<Contour>(0);
+        }
+
+        public bool Equals(Polygon other)
+        {
+            return other == this;
+        }
+
+        public void AddAsChild(Polygon other)
+        {
+            this.Holes.Add(other.hull);
+            this.Holes.AddRange(other.Holes);
+        }
+
+        public int TotalVertCount()
+        {
+            int result = hull.VertexCount;
+            foreach (var path in Holes)
+            {
+                result += path.VertexCount;
+            }
+            return result;
+        }
+
+        public bool BoundsOverlap(Polygon other)
+        {
+            return boundingRect.Overlaps(other.boundingRect);
+        }
+
+        public bool PointInPolyon(Vector2 point)
+        {
+            return boundingRect.Contains(point) && hull.PointInContour(point);
+        }
+
+        public bool Contains(Polygon other)
+        {
+            foreach (var v in other.hull)
+            {
+                if (!PointInPolyon(v))
+                    return false;
+            }
+            return true;
+        }
+
+        public bool Contains(Contour other)
+        {
+            foreach (var v in other)
+            {
+                if (!PointInPolyon(v))
+                    return false;
+            }
+            return true;
+        }
+
+        public void Draw()
+        {
+            Gizmos.color = Color.green;
+            hull.Draw();
+
+            Gizmos.color = Color.yellow;
+            foreach (var contour in this.Holes)
+            {
+                contour.Draw();
+            }
+        }
+
+        public IEnumerator<Contour> GetEnumerator()
+        {
+            yield return hull;
+            foreach (var hole in Holes)
+                yield return hole;
+        }
+
+        IEnumerator IEnumerable.GetEnumerator()
+        {
+            yield return hull;
+            foreach (var hole in Holes)
+                yield return hole;
+        }
+
+        public void UpdateBounds()
+        {
+            if (hull.VertexCount == 0)
+                return;
+
+            Vector2 min = hull[0];
+            Vector2 max = hull[0];
+            for (int iVert = 1; iVert < hull.VertexCount; iVert++)
+            {
+                min = Vector2.Min(hull[iVert], min);
+                max = Vector2.Max(hull[iVert], max);
+            }
+            // add some fudge
+            Vector2 fudge = new Vector2(0.0001f, 0.0001f);
+            boundingRect = new Rect(min - fudge, max - min + fudge * 2);
+        }
+
+        public void Simplify(float tolerance)
+        {
+            hull.Simplify(tolerance);
+            foreach (var hole in Holes)
+            {
+                hole.Simplify(tolerance);
+            }
+            UpdateBounds();
+        }
+
+        public double SignedArea()
+        {
+            double area = Hull.SignedArea();
+            foreach (var hole in Holes)
+                area += hole.SignedArea();
+            return area;
+        }
+
+        public void EnsureCWOrdering()
+        {
+            if (Hull.IsCW())
+                return;
+
+            Hull.Invert();
+            foreach (var child in Holes)
+                child.Invert();
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/Clipping/Polygon.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 24eb0a7c0c44eff48b61ea1d3120dd18
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/CustomAttributes.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 81309357b39e41942b7819480b292f4a
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/CustomAttributes/ReadOnlyAttribute.cs

@@ -0,0 +1,9 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    internal class ReadOnlyAttribute : PropertyAttribute
+    {
+
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/CustomAttributes/ReadOnlyAttribute.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 334716afcf3386f4b8200ed669e80282
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/CustomAttributes/ScriptExecutionOrderAttribute.cs

@@ -0,0 +1,20 @@
+using System;
+
+namespace PathBerserker2d
+{
+    [AttributeUsage(AttributeTargets.Class)]
+    internal class ScriptExecutionOrderAttribute : Attribute
+    {
+        private int order = 0;
+
+        public ScriptExecutionOrderAttribute(int order)
+        {
+            this.order = order;
+        }
+
+        public int GetOrder()
+        {
+            return order;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/CustomAttributes/ScriptExecutionOrderAttribute.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 036645a6650ecf0478b961d469c7e935
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 68e576ad0cfa64743a23dd8e8db94b39
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/ExternalSortingFastPriorityQueueNode.cs

@@ -0,0 +1,12 @@
+namespace Priority_Queue
+{
+    internal abstract class ExternalSortingFastPriorityQueueNode : System.IComparable<ExternalSortingFastPriorityQueueNode>
+    {
+        /// <summary>
+        /// Represents the current position in the queue
+        /// </summary>
+        public int QueueIndex { get; internal set; }
+
+        public abstract int CompareTo(ExternalSortingFastPriorityQueueNode other);
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/ExternalSortingFastPriorityQueueNode.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 79ff18f1f5314034a980f4c7a9a66d56
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/FastPriorityQueue.cs

@@ -0,0 +1,418 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Runtime.CompilerServices;
+
+namespace Priority_Queue
+{
+    /// <summary>
+    /// An implementation of a min-Priority Queue using a heap.  Has O(1) .Contains()!
+    /// See https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp/wiki/Getting-Started for more information
+    /// </summary>
+    /// <typeparam name="T">The values in the queue.  Must extend the FastPriorityQueueNode class</typeparam>
+    internal sealed class FastPriorityQueue<T> : IFixedSizePriorityQueue<T>
+        where T : FastPriorityQueueNode
+    {
+        private int _numNodes;
+        private T[] _nodes;
+
+        /// <summary>
+        /// Instantiate a new Priority Queue
+        /// </summary>
+        /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
+        public FastPriorityQueue(int maxNodes)
+        {
+#if PBDEBUG
+            if (maxNodes <= 0)
+            {
+                throw new InvalidOperationException("New queue size cannot be smaller than 1");
+            }
+#endif
+
+            _numNodes = 0;
+            _nodes = new T[maxNodes + 1];
+        }
+
+        /// <summary>
+        /// Returns the number of nodes in the queue.
+        /// O(1)
+        /// </summary>
+        public int Count
+        {
+            get
+            {
+                return _numNodes;
+            }
+        }
+
+        /// <summary>
+        /// Returns the maximum number of items that can be enqueued at once in this queue.  Once you hit this number (ie. once Count == MaxSize),
+        /// attempting to enqueue another item will cause undefined behavior.  O(1)
+        /// </summary>
+        public int MaxSize
+        {
+            get
+            {
+                return _nodes.Length - 1;
+            }
+        }
+
+        /// <summary>
+        /// Removes every node from the queue.
+        /// O(n) (So, don't do this often!)
+        /// </summary>
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        public void Clear()
+        {
+            Array.Clear(_nodes, 1, _numNodes);
+            _numNodes = 0;
+        }
+
+        /// <summary>
+        /// Returns (in O(1)!) whether the given node is in the queue.  O(1)
+        /// </summary>
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        public bool Contains(T node)
+        {
+#if PBDEBUG
+            if (node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length)
+            {
+                throw new InvalidOperationException("node.QueueIndex has been corrupted. Did you change it manually? Or add this node to another queue?");
+            }
+#endif
+
+            return (_nodes[node.QueueIndex] == node);
+        }
+
+        /// <summary>
+        /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
+        /// If the queue is full, the result is undefined.
+        /// If the node is already enqueued, the result is undefined.
+        /// O(log n)
+        /// </summary>
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        public void Enqueue(T node, float priority)
+        {
+#if PBDEBUG
+            if (node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if (_numNodes >= _nodes.Length - 1)
+            {
+                throw new InvalidOperationException("Queue is full - node cannot be added: " + node);
+            }
+            if (Contains(node))
+            {
+                throw new InvalidOperationException("Node is already enqueued: " + node);
+            }
+#endif
+
+            node.Priority = priority;
+            _numNodes++;
+            _nodes[_numNodes] = node;
+            node.QueueIndex = _numNodes;
+            CascadeUp(_nodes[_numNodes]);
+        }
+
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        private void Swap(T node1, T node2)
+        {
+            //Swap the nodes
+            _nodes[node1.QueueIndex] = node2;
+            _nodes[node2.QueueIndex] = node1;
+
+            //Swap their indicies
+            int temp = node1.QueueIndex;
+            node1.QueueIndex = node2.QueueIndex;
+            node2.QueueIndex = temp;
+        }
+
+        //Performance appears to be slightly better when this is NOT inlined o_O
+        private void CascadeUp(T node)
+        {
+            //aka Heapify-up
+            int parent = node.QueueIndex / 2;
+            while (parent >= 1)
+            {
+                T parentNode = _nodes[parent];
+                if (HasHigherPriority(parentNode, node))
+                    break;
+
+                //Node has lower priority value, so move it up the heap
+                Swap(node, parentNode); //For some reason, this is faster with Swap() rather than (less..?) individual operations, like in CascadeDown()
+
+                parent = node.QueueIndex / 2;
+            }
+        }
+
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        private void CascadeDown(T node)
+        {
+            //aka Heapify-down
+            T newParent;
+            int finalQueueIndex = node.QueueIndex;
+            while (true)
+            {
+                newParent = node;
+                int childLeftIndex = 2 * finalQueueIndex;
+
+                //Check if the left-child is higher-priority than the current node
+                if (childLeftIndex > _numNodes)
+                {
+                    //This could be placed outside the loop, but then we'd have to check newParent != node twice
+                    node.QueueIndex = finalQueueIndex;
+                    _nodes[finalQueueIndex] = node;
+                    break;
+                }
+
+                T childLeft = _nodes[childLeftIndex];
+                if (HasHigherPriority(childLeft, newParent))
+                {
+                    newParent = childLeft;
+                }
+
+                //Check if the right-child is higher-priority than either the current node or the left child
+                int childRightIndex = childLeftIndex + 1;
+                if (childRightIndex <= _numNodes)
+                {
+                    T childRight = _nodes[childRightIndex];
+                    if (HasHigherPriority(childRight, newParent))
+                    {
+                        newParent = childRight;
+                    }
+                }
+
+                //If either of the children has higher (smaller) priority, swap and continue cascading
+                if (newParent != node)
+                {
+                    //Move new parent to its new index.  node will be moved once, at the end
+                    //Doing it this way is one less assignment operation than calling Swap()
+                    _nodes[finalQueueIndex] = newParent;
+
+                    int temp = newParent.QueueIndex;
+                    newParent.QueueIndex = finalQueueIndex;
+                    finalQueueIndex = temp;
+                }
+                else
+                {
+                    //See note above
+                    node.QueueIndex = finalQueueIndex;
+                    _nodes[finalQueueIndex] = node;
+                    break;
+                }
+            }
+        }
+
+        /// <summary>
+        /// Returns true if 'higher' has higher priority than 'lower', false otherwise.
+        /// Note that calling HasHigherPriority(node, node) (ie. both arguments the same node) will return false
+        /// </summary>
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        private bool HasHigherPriority(T higher, T lower)
+        {
+            return (higher.Priority < lower.Priority);
+        }
+
+        /// <summary>
+        /// Removes the head of the queue and returns it.
+        /// If queue is empty, result is undefined
+        /// O(log n)
+        /// </summary>
+        public T Dequeue()
+        {
+#if PBDEBUG
+            if (_numNodes <= 0)
+            {
+                throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
+            }
+
+            if (!IsValidQueue())
+            {
+                throw new InvalidOperationException("Queue has been corrupted (Did you update a node priority manually instead of calling UpdatePriority()?" +
+                                                    "Or add the same node to two different queues?)");
+            }
+#endif
+
+            T returnMe = _nodes[1];
+            Remove(returnMe);
+            return returnMe;
+        }
+
+        /// <summary>
+        /// Resize the queue so it can accept more nodes.  All currently enqueued nodes are remain.
+        /// Attempting to decrease the queue size to a size too small to hold the existing nodes results in undefined behavior
+        /// O(n)
+        /// </summary>
+        public void Resize(int maxNodes)
+        {
+#if PBDEBUG
+            if (maxNodes <= 0)
+            {
+                throw new InvalidOperationException("Queue size cannot be smaller than 1");
+            }
+
+            if (maxNodes < _numNodes)
+            {
+                throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes");
+            }
+#endif
+
+            T[] newArray = new T[maxNodes + 1];
+            int highestIndexToCopy = Math.Min(maxNodes, _numNodes);
+            for (int i = 1; i <= highestIndexToCopy; i++)
+            {
+                newArray[i] = _nodes[i];
+            }
+            _nodes = newArray;
+        }
+
+        /// <summary>
+        /// Returns the head of the queue, without removing it (use Dequeue() for that).
+        /// If the queue is empty, behavior is undefined.
+        /// O(1)
+        /// </summary>
+        public T First
+        {
+            get
+            {
+#if PBDEBUG
+                if (_numNodes <= 0)
+                {
+                    throw new InvalidOperationException("Cannot call .First on an empty queue");
+                }
+#endif
+
+                return _nodes[1];
+            }
+        }
+
+        /// <summary>
+        /// This method must be called on a node every time its priority changes while it is in the queue.  
+        /// <b>Forgetting to call this method will result in a corrupted queue!</b>
+        /// Calling this method on a node not in the queue results in undefined behavior
+        /// O(log n)
+        /// </summary>
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        public void UpdatePriority(T node, float priority)
+        {
+#if PBDEBUG
+            if (node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if (!Contains(node))
+            {
+                throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + node);
+            }
+#endif
+
+            node.Priority = priority;
+            OnNodeUpdated(node);
+        }
+
+        private void OnNodeUpdated(T node)
+        {
+            //Bubble the updated node up or down as appropriate
+            int parentIndex = node.QueueIndex / 2;
+            T parentNode = _nodes[parentIndex];
+
+            if (parentIndex > 0 && HasHigherPriority(node, parentNode))
+            {
+                CascadeUp(node);
+            }
+            else
+            {
+                //Note that CascadeDown will be called if parentNode == node (that is, node is the root)
+                CascadeDown(node);
+            }
+        }
+
+        /// <summary>
+        /// Removes a node from the queue.  The node does not need to be the head of the queue.  
+        /// If the node is not in the queue, the result is undefined.  If unsure, check Contains() first
+        /// O(log n)
+        /// </summary>
+        public void Remove(T node)
+        {
+#if PBDEBUG
+            if (node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if (!Contains(node))
+            {
+                throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + node);
+            }
+#endif
+
+            //If the node is already the last node, we can remove it immediately
+            if (node.QueueIndex == _numNodes)
+            {
+                _nodes[_numNodes] = null;
+                _numNodes--;
+                return;
+            }
+
+            //Swap the node with the last node
+            T formerLastNode = _nodes[_numNodes];
+            Swap(node, formerLastNode);
+            _nodes[_numNodes] = null;
+            _numNodes--;
+
+            //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate
+            OnNodeUpdated(formerLastNode);
+        }
+
+        public IEnumerator<T> GetEnumerator()
+        {
+            for (int i = 1; i <= _numNodes; i++)
+                yield return _nodes[i];
+        }
+
+        IEnumerator IEnumerable.GetEnumerator()
+        {
+            return GetEnumerator();
+        }
+
+        /// <summary>
+        /// <b>Should not be called in production code.</b>
+        /// Checks to make sure the queue is still in a valid state.  Used for testing/debugging the queue.
+        /// </summary>
+        public bool IsValidQueue()
+        {
+            for (int i = 1; i < _nodes.Length; i++)
+            {
+                if (_nodes[i] != null)
+                {
+                    int childLeftIndex = 2 * i;
+                    if (childLeftIndex < _nodes.Length && _nodes[childLeftIndex] != null && HasHigherPriority(_nodes[childLeftIndex], _nodes[i]))
+                        return false;
+
+                    int childRightIndex = childLeftIndex + 1;
+                    if (childRightIndex < _nodes.Length && _nodes[childRightIndex] != null && HasHigherPriority(_nodes[childRightIndex], _nodes[i]))
+                        return false;
+                }
+            }
+            return true;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/FastPriorityQueue.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 376b08cc68eb144468648ebcd4a478e6
+timeCreated: 1476131089
+licenseType: Store
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/FastPriorityQueueNode.cs

@@ -0,0 +1,16 @@
+namespace Priority_Queue
+{
+    internal class FastPriorityQueueNode
+    {
+        /// <summary>
+        /// The Priority to insert this node at.  Must be set BEFORE adding a node to the queue (ideally just once, in the node's constructor).
+        /// Should not be manually edited once the node has been enqueued - use queue.UpdatePriority() instead
+        /// </summary>
+        public float Priority { get; protected internal set; }
+
+        /// <summary>
+        /// Represents the current position in the queue
+        /// </summary>
+        public int QueueIndex { get; internal set; }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/FastPriorityQueueNode.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 4bb5e6cfc638aca43a63a7304f2fe754
+timeCreated: 1476131089
+licenseType: Store
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/IFixedSizePriorityQueue.cs

@@ -0,0 +1,24 @@
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+namespace Priority_Queue
+{
+    /// <summary>
+    /// A helper-interface only needed to make writing unit tests a bit easier (hence the 'internal' access modifier)
+    /// </summary>
+    internal interface IFixedSizePriorityQueue<TItem> : IPriorityQueue<TItem>
+    {
+        /// <summary>
+        /// Resize the queue so it can accept more nodes.  All currently enqueued nodes are remain.
+        /// Attempting to decrease the queue size to a size too small to hold the existing nodes results in undefined behavior
+        /// </summary>
+        void Resize(int maxNodes);
+
+        /// <summary>
+        /// Returns the maximum number of items that can be enqueued at once in this queue.  Once you hit this number (ie. once Count == MaxSize),
+        /// attempting to enqueue another item will cause undefined behavior.
+        /// </summary>
+        int MaxSize { get; }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/IFixedSizePriorityQueue.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: d9cacd2c305e5a94dbfeba14e25a0441
+timeCreated: 1476131089
+licenseType: Store
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/IPriorityQueue.cs

@@ -0,0 +1,53 @@
+using System.Collections.Generic;
+
+namespace Priority_Queue
+{
+    /// <summary>
+    /// The IPriorityQueue interface.  This is mainly here for purists, and in case I decide to add more implementations later.
+    /// For speed purposes, it is actually recommended that you *don't* access the priority queue through this interface, since the JIT can
+    /// (theoretically?) optimize method calls from concrete-types slightly better.
+    /// </summary>
+    internal interface IPriorityQueue<T> : IEnumerable<T>
+    {
+        /// <summary>
+        /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
+        /// See implementation for how duplicates are handled.
+        /// </summary>
+        void Enqueue(T node, float priority);
+
+        /// <summary>
+        /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it.
+        /// </summary>
+        T Dequeue();
+
+        /// <summary>
+        /// Removes every node from the queue.
+        /// </summary>
+        void Clear();
+
+        /// <summary>
+        /// Returns whether the given node is in the queue.
+        /// </summary>
+        bool Contains(T node);
+
+        /// <summary>
+        /// Removes a node from the queue.  The node does not need to be the head of the queue.  
+        /// </summary>
+        void Remove(T node);
+
+        /// <summary>
+        /// Call this method to change the priority of a node.  
+        /// </summary>
+        void UpdatePriority(T node, float priority);
+
+        /// <summary>
+        /// Returns the head of the queue, without removing it (use Dequeue() for that).
+        /// </summary>
+        T First { get; }
+
+        /// <summary>
+        /// Returns the number of nodes in the queue.
+        /// </summary>
+        int Count { get; }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/IPriorityQueue.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 722bac7805cf3b74590b5eecfe9ff7f0
+timeCreated: 1476131089
+licenseType: Store
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/SimplePriorityQueue.cs

@@ -0,0 +1,229 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+
+namespace Priority_Queue
+{
+    /// <summary>
+    /// A simplified priority queue implementation.  Is stable, auto-resizes, and thread-safe, at the cost of being slightly slower than
+    /// FastPriorityQueue
+    /// </summary>
+    /// <typeparam name="T">The type to enqueue</typeparam>
+    internal sealed class SimplePriorityQueue<T> : IPriorityQueue<T>
+    {
+        private class SimpleNode : StablePriorityQueueNode
+        {
+            public T Data { get; private set; }
+
+            public SimpleNode(T data)
+            {
+                Data = data;
+            }
+        }
+
+        private const int INITIAL_QUEUE_SIZE = 10;
+        private readonly StablePriorityQueue<SimpleNode> _queue;
+
+        public SimplePriorityQueue()
+        {
+            _queue = new StablePriorityQueue<SimpleNode>(INITIAL_QUEUE_SIZE);
+        }
+
+        /// <summary>
+        /// Given an item of type T, returns the exist SimpleNode in the queue
+        /// </summary>
+        private SimpleNode GetExistingNode(T item)
+        {
+            var comparer = EqualityComparer<T>.Default;
+            foreach (var node in _queue)
+            {
+                if (comparer.Equals(node.Data, item))
+                {
+                    return node;
+                }
+            }
+            throw new InvalidOperationException("Item cannot be found in queue: " + item);
+        }
+
+        /// <summary>
+        /// Returns the number of nodes in the queue.
+        /// O(1)
+        /// </summary>
+        public int Count
+        {
+            get
+            {
+                lock (_queue)
+                {
+                    return _queue.Count;
+                }
+            }
+        }
+
+
+        /// <summary>
+        /// Returns the head of the queue, without removing it (use Dequeue() for that).
+        /// Throws an exception when the queue is empty.
+        /// O(1)
+        /// </summary>
+        public T First
+        {
+            get
+            {
+                lock (_queue)
+                {
+                    if (_queue.Count <= 0)
+                    {
+                        throw new InvalidOperationException("Cannot call .First on an empty queue");
+                    }
+
+                    SimpleNode first = _queue.First;
+                    return (first != null ? first.Data : default(T));
+                }
+            }
+        }
+
+        /// <summary>
+        /// Removes every node from the queue.
+        /// O(n)
+        /// </summary>
+        public void Clear()
+        {
+            lock (_queue)
+            {
+                _queue.Clear();
+            }
+        }
+
+        /// <summary>
+        /// Returns whether the given item is in the queue.
+        /// O(n)
+        /// </summary>
+        public bool Contains(T item)
+        {
+            lock (_queue)
+            {
+                var comparer = EqualityComparer<T>.Default;
+                foreach (var node in _queue)
+                {
+                    if (comparer.Equals(node.Data, item))
+                    {
+                        return true;
+                    }
+                }
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it.
+        /// If queue is empty, throws an exception
+        /// O(log n)
+        /// </summary>
+        public T Dequeue()
+        {
+            lock (_queue)
+            {
+                if (_queue.Count <= 0)
+                {
+                    throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
+                }
+
+                SimpleNode node = _queue.Dequeue();
+                return node.Data;
+            }
+        }
+
+        /// <summary>
+        /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
+        /// This queue automatically resizes itself, so there's no concern of the queue becoming 'full'.
+        /// Duplicates are allowed.
+        /// O(log n)
+        /// </summary>
+        public void Enqueue(T item, float priority)
+        {
+            lock (_queue)
+            {
+                SimpleNode node = new SimpleNode(item);
+                if (_queue.Count == _queue.MaxSize)
+                {
+                    _queue.Resize(_queue.MaxSize * 2 + 1);
+                }
+                _queue.Enqueue(node, priority);
+            }
+        }
+
+        /// <summary>
+        /// Removes an item from the queue.  The item does not need to be the head of the queue.  
+        /// If the item is not in the queue, an exception is thrown.  If unsure, check Contains() first.
+        /// If multiple copies of the item are enqueued, only the first one is removed. 
+        /// O(n)
+        /// </summary>
+        public void Remove(T item)
+        {
+            lock (_queue)
+            {
+                try
+                {
+                    _queue.Remove(GetExistingNode(item));
+                }
+                catch (InvalidOperationException ex)
+                {
+                    throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + item, ex);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Call this method to change the priority of an item.
+        /// Calling this method on a item not in the queue will throw an exception.
+        /// If the item is enqueued multiple times, only the first one will be updated.
+        /// (If your requirements are complex enough that you need to enqueue the same item multiple times <i>and</i> be able
+        /// to update all of them, please wrap your items in a wrapper class so they can be distinguished).
+        /// O(n)
+        /// </summary>
+        public void UpdatePriority(T item, float priority)
+        {
+            lock (_queue)
+            {
+                try
+                {
+                    SimpleNode updateMe = GetExistingNode(item);
+                    _queue.UpdatePriority(updateMe, priority);
+                }
+                catch (InvalidOperationException ex)
+                {
+                    throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + item, ex);
+                }
+            }
+        }
+
+        public IEnumerator<T> GetEnumerator()
+        {
+            List<T> queueData = new List<T>();
+            lock (_queue)
+            {
+                //Copy to a separate list because we don't want to 'yield return' inside a lock
+                foreach (var node in _queue)
+                {
+                    queueData.Add(node.Data);
+                }
+            }
+
+            return queueData.GetEnumerator();
+        }
+
+        IEnumerator IEnumerable.GetEnumerator()
+        {
+            return GetEnumerator();
+        }
+
+        public bool IsValidQueue()
+        {
+            lock (_queue)
+            {
+                return _queue.IsValidQueue();
+            }
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/SimplePriorityQueue.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 1daaf3be153500e4d80b80fe17ccc211
+timeCreated: 1476131088
+licenseType: Store
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/StablePriorityQueue.cs

@@ -0,0 +1,423 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Runtime.CompilerServices;
+
+namespace Priority_Queue
+{
+    /// <summary>
+    /// A copy of FastPriorityQueue which is also stable - that is, when two nodes are enqueued with the same priority, they
+    /// are always dequeued in the same order.
+    /// See https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp/wiki/Getting-Started for more information
+    /// </summary>
+    /// <typeparam name="T">The values in the queue.  Must extend the StablePriorityQueueNode class</typeparam>
+    internal sealed class StablePriorityQueue<T> : IFixedSizePriorityQueue<T>
+        where T : StablePriorityQueueNode
+    {
+        private int _numNodes;
+        private T[] _nodes;
+        private long _numNodesEverEnqueued;
+
+        /// <summary>
+        /// Instantiate a new Priority Queue
+        /// </summary>
+        /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
+        public StablePriorityQueue(int maxNodes)
+        {
+#if PBDEBUG
+            if (maxNodes <= 0)
+            {
+                throw new InvalidOperationException("New queue size cannot be smaller than 1");
+            }
+#endif
+
+            _numNodes = 0;
+            _nodes = new T[maxNodes + 1];
+            _numNodesEverEnqueued = 0;
+        }
+
+        /// <summary>
+        /// Returns the number of nodes in the queue.
+        /// O(1)
+        /// </summary>
+        public int Count
+        {
+            get
+            {
+                return _numNodes;
+            }
+        }
+
+        /// <summary>
+        /// Returns the maximum number of items that can be enqueued at once in this queue.  Once you hit this number (ie. once Count == MaxSize),
+        /// attempting to enqueue another item will cause undefined behavior.  O(1)
+        /// </summary>
+        public int MaxSize
+        {
+            get
+            {
+                return _nodes.Length - 1;
+            }
+        }
+
+        /// <summary>
+        /// Removes every node from the queue.
+        /// O(n) (So, don't do this often!)
+        /// </summary>
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        public void Clear()
+        {
+            Array.Clear(_nodes, 1, _numNodes);
+            _numNodes = 0;
+        }
+
+        /// <summary>
+        /// Returns (in O(1)!) whether the given node is in the queue.  O(1)
+        /// </summary>
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        public bool Contains(T node)
+        {
+#if PBDEBUG
+            if (node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if (node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length)
+            {
+                throw new InvalidOperationException("node.QueueIndex has been corrupted. Did you change it manually? Or add this node to another queue?");
+            }
+#endif
+
+            return (_nodes[node.QueueIndex] == node);
+        }
+
+        /// <summary>
+        /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
+        /// If the queue is full, the result is undefined.
+        /// If the node is already enqueued, the result is undefined.
+        /// O(log n)
+        /// </summary>
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        public void Enqueue(T node, float priority)
+        {
+#if PBDEBUG
+            if (node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if (_numNodes >= _nodes.Length - 1)
+            {
+                throw new InvalidOperationException("Queue is full - node cannot be added: " + node);
+            }
+            if (Contains(node))
+            {
+                throw new InvalidOperationException("Node is already enqueued: " + node);
+            }
+#endif
+
+            node.Priority = priority;
+            _numNodes++;
+            _nodes[_numNodes] = node;
+            node.QueueIndex = _numNodes;
+            node.InsertionIndex = _numNodesEverEnqueued++;
+            CascadeUp(_nodes[_numNodes]);
+        }
+
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        private void Swap(T node1, T node2)
+        {
+            //Swap the nodes
+            _nodes[node1.QueueIndex] = node2;
+            _nodes[node2.QueueIndex] = node1;
+
+            //Swap their indicies
+            int temp = node1.QueueIndex;
+            node1.QueueIndex = node2.QueueIndex;
+            node2.QueueIndex = temp;
+        }
+
+        //Performance appears to be slightly better when this is NOT inlined o_O
+        private void CascadeUp(T node)
+        {
+            //aka Heapify-up
+            int parent = node.QueueIndex / 2;
+            while (parent >= 1)
+            {
+                T parentNode = _nodes[parent];
+                if (HasHigherPriority(parentNode, node))
+                    break;
+
+                //Node has lower priority value, so move it up the heap
+                Swap(node, parentNode); //For some reason, this is faster with Swap() rather than (less..?) individual operations, like in CascadeDown()
+
+                parent = node.QueueIndex / 2;
+            }
+        }
+
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        private void CascadeDown(T node)
+        {
+            //aka Heapify-down
+            T newParent;
+            int finalQueueIndex = node.QueueIndex;
+            while (true)
+            {
+                newParent = node;
+                int childLeftIndex = 2 * finalQueueIndex;
+
+                //Check if the left-child is higher-priority than the current node
+                if (childLeftIndex > _numNodes)
+                {
+                    //This could be placed outside the loop, but then we'd have to check newParent != node twice
+                    node.QueueIndex = finalQueueIndex;
+                    _nodes[finalQueueIndex] = node;
+                    break;
+                }
+
+                T childLeft = _nodes[childLeftIndex];
+                if (HasHigherPriority(childLeft, newParent))
+                {
+                    newParent = childLeft;
+                }
+
+                //Check if the right-child is higher-priority than either the current node or the left child
+                int childRightIndex = childLeftIndex + 1;
+                if (childRightIndex <= _numNodes)
+                {
+                    T childRight = _nodes[childRightIndex];
+                    if (HasHigherPriority(childRight, newParent))
+                    {
+                        newParent = childRight;
+                    }
+                }
+
+                //If either of the children has higher (smaller) priority, swap and continue cascading
+                if (newParent != node)
+                {
+                    //Move new parent to its new index.  node will be moved once, at the end
+                    //Doing it this way is one less assignment operation than calling Swap()
+                    _nodes[finalQueueIndex] = newParent;
+
+                    int temp = newParent.QueueIndex;
+                    newParent.QueueIndex = finalQueueIndex;
+                    finalQueueIndex = temp;
+                }
+                else
+                {
+                    //See note above
+                    node.QueueIndex = finalQueueIndex;
+                    _nodes[finalQueueIndex] = node;
+                    break;
+                }
+            }
+        }
+
+        /// <summary>
+        /// Returns true if 'higher' has higher priority than 'lower', false otherwise.
+        /// Note that calling HasHigherPriority(node, node) (ie. both arguments the same node) will return false
+        /// </summary>
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        private bool HasHigherPriority(T higher, T lower)
+        {
+            return (higher.Priority < lower.Priority ||
+                (higher.Priority == lower.Priority && higher.InsertionIndex < lower.InsertionIndex));
+        }
+
+        /// <summary>
+        /// Removes the head of the queue (node with minimum priority; ties are broken by order of insertion), and returns it.
+        /// If queue is empty, result is undefined
+        /// O(log n)
+        /// </summary>
+        public T Dequeue()
+        {
+#if PBDEBUG
+            if (_numNodes <= 0)
+            {
+                throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
+            }
+
+            if (!IsValidQueue())
+            {
+                throw new InvalidOperationException("Queue has been corrupted (Did you update a node priority manually instead of calling UpdatePriority()?" +
+                                                    "Or add the same node to two different queues?)");
+            }
+#endif
+
+            T returnMe = _nodes[1];
+            Remove(returnMe);
+            return returnMe;
+        }
+
+        /// <summary>
+        /// Resize the queue so it can accept more nodes.  All currently enqueued nodes are remain.
+        /// Attempting to decrease the queue size to a size too small to hold the existing nodes results in undefined behavior
+        /// O(n)
+        /// </summary>
+        public void Resize(int maxNodes)
+        {
+#if PBDEBUG
+            if (maxNodes <= 0)
+            {
+                throw new InvalidOperationException("Queue size cannot be smaller than 1");
+            }
+
+            if (maxNodes < _numNodes)
+            {
+                throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes");
+            }
+#endif
+
+            T[] newArray = new T[maxNodes + 1];
+            int highestIndexToCopy = Math.Min(maxNodes, _numNodes);
+            for (int i = 1; i <= highestIndexToCopy; i++)
+            {
+                newArray[i] = _nodes[i];
+            }
+            _nodes = newArray;
+        }
+
+        /// <summary>
+        /// Returns the head of the queue, without removing it (use Dequeue() for that).
+        /// If the queue is empty, behavior is undefined.
+        /// O(1)
+        /// </summary>
+        public T First
+        {
+            get
+            {
+#if PBDEBUG
+                if (_numNodes <= 0)
+                {
+                    throw new InvalidOperationException("Cannot call .First on an empty queue");
+                }
+#endif
+
+                return _nodes[1];
+            }
+        }
+
+        /// <summary>
+        /// This method must be called on a node every time its priority changes while it is in the queue.  
+        /// <b>Forgetting to call this method will result in a corrupted queue!</b>
+        /// Calling this method on a node not in the queue results in undefined behavior
+        /// O(log n)
+        /// </summary>
+#if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+#endif
+        public void UpdatePriority(T node, float priority)
+        {
+#if PBDEBUG
+            if (node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if (!Contains(node))
+            {
+                throw new InvalidOperationException("Cannot call UpdatePriority() on a node which is not enqueued: " + node);
+            }
+#endif
+
+            node.Priority = priority;
+            OnNodeUpdated(node);
+        }
+
+        private void OnNodeUpdated(T node)
+        {
+            //Bubble the updated node up or down as appropriate
+            int parentIndex = node.QueueIndex / 2;
+            T parentNode = _nodes[parentIndex];
+
+            if (parentIndex > 0 && HasHigherPriority(node, parentNode))
+            {
+                CascadeUp(node);
+            }
+            else
+            {
+                //Note that CascadeDown will be called if parentNode == node (that is, node is the root)
+                CascadeDown(node);
+            }
+        }
+
+        /// <summary>
+        /// Removes a node from the queue.  The node does not need to be the head of the queue.  
+        /// If the node is not in the queue, the result is undefined.  If unsure, check Contains() first
+        /// O(log n)
+        /// </summary>
+        public void Remove(T node)
+        {
+#if PBDEBUG
+            if (node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if (!Contains(node))
+            {
+                throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + node);
+            }
+#endif
+
+            //If the node is already the last node, we can remove it immediately
+            if (node.QueueIndex == _numNodes)
+            {
+                _nodes[_numNodes] = null;
+                _numNodes--;
+                return;
+            }
+
+            //Swap the node with the last node
+            T formerLastNode = _nodes[_numNodes];
+            Swap(node, formerLastNode);
+            _nodes[_numNodes] = null;
+            _numNodes--;
+
+            //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate
+            OnNodeUpdated(formerLastNode);
+        }
+
+        public IEnumerator<T> GetEnumerator()
+        {
+            for (int i = 1; i <= _numNodes; i++)
+                yield return _nodes[i];
+        }
+
+        IEnumerator IEnumerable.GetEnumerator()
+        {
+            return GetEnumerator();
+        }
+
+        /// <summary>
+        /// <b>Should not be called in production code.</b>
+        /// Checks to make sure the queue is still in a valid state.  Used for testing/debugging the queue.
+        /// </summary>
+        public bool IsValidQueue()
+        {
+            for (int i = 1; i < _nodes.Length; i++)
+            {
+                if (_nodes[i] != null)
+                {
+                    int childLeftIndex = 2 * i;
+                    if (childLeftIndex < _nodes.Length && _nodes[childLeftIndex] != null && HasHigherPriority(_nodes[childLeftIndex], _nodes[i]))
+                        return false;
+
+                    int childRightIndex = childLeftIndex + 1;
+                    if (childRightIndex < _nodes.Length && _nodes[childRightIndex] != null && HasHigherPriority(_nodes[childRightIndex], _nodes[i]))
+                        return false;
+                }
+            }
+            return true;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/StablePriorityQueue.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 9e4182f1acbb9fc40b5e9356cebb9685
+timeCreated: 1476131089
+licenseType: Store
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/StablePriorityQueueNode.cs

@@ -0,0 +1,10 @@
+namespace Priority_Queue
+{
+    internal class StablePriorityQueueNode : FastPriorityQueueNode
+    {
+        /// <summary>
+        /// Represents the order the node was inserted in
+        /// </summary>
+        public long InsertionIndex { get; internal set; }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/StablePriorityQueueNode.cs.meta

@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 316bff9a2da34f5489becd70b0065188
+timeCreated: 1476131088
+licenseType: Store
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/SweepEventPriorityQueue.cs

@@ -0,0 +1,386 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Runtime.CompilerServices;
+using static PathBerserker2d.PolygonClipper;
+
+namespace Priority_Queue
+{
+    /// <summary>
+    /// An implementation of a min-Priority Queue using a heap.  Has O(1) .Contains()!
+    /// See https://github.com/BlueRaja/High-Speed-Priority-Queue-for-C-Sharp/wiki/Getting-Started for more information
+    /// </summary>
+    internal sealed class SweepEventPriorityQueue
+    {
+        private int _numNodes;
+        private SweepEvent[] _nodes;
+
+        /// <summary>
+        /// Instantiate a new Priority Queue
+        /// </summary>
+        /// <param name="maxNodes">The max nodes ever allowed to be enqueued (going over this will cause undefined behavior)</param>
+        public SweepEventPriorityQueue(int maxNodes)
+        {
+            #if QUEUE_DEBUG
+            if (maxNodes <= 0)
+            {
+                throw new InvalidOperationException("New queue size cannot be smaller than 1");
+            }
+            #endif
+
+            _numNodes = 0;
+            _nodes = new SweepEvent[maxNodes + 1];
+        }
+
+        /// <summary>
+        /// Returns the number of nodes in the queue.
+        /// O(1)
+        /// </summary>
+        public int Count
+        {
+            get
+            {
+                return _numNodes;
+            }
+        }
+
+        /// <summary>
+        /// Returns the maximum number of items that can be enqueued at once in this queue.  Once you hit this number (ie. once Count == MaxSize),
+        /// attempting to enqueue another item will cause undefined behavior.  O(1)
+        /// </summary>
+        public int MaxSize
+        {
+            get
+            {
+                return _nodes.Length - 1;
+            }
+        }
+
+        /// <summary>
+        /// Removes every node from the queue.
+        /// O(n) (So, don't do this often!)
+        /// </summary>
+        #if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        #endif
+        public void Clear()
+        {
+            Array.Clear(_nodes, 1, _numNodes);
+            _numNodes = 0;
+        }
+
+        /// <summary>
+        /// Returns (in O(1)!) whether the given node is in the queue.  O(1)
+        /// </summary>
+        #if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        #endif
+        public bool Contains(SweepEvent node)
+        {
+#if QUEUE_DEBUG
+            if(node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if(node.QueueIndex < 0 || node.QueueIndex >= _nodes.Length)
+            {
+                throw new InvalidOperationException("node.QueueIndex has been corrupted. Did you change it manually? Or add this node to another queue?");
+            }
+#endif
+
+            return (_nodes[node.queueIndex] == node);
+        }
+
+        /// <summary>
+        /// Enqueue a node to the priority queue.  Lower values are placed in front. Ties are broken by first-in-first-out.
+        /// If the queue is full, the result is undefined.
+        /// If the node is already enqueued, the result is undefined.
+        /// O(log n)
+        /// </summary>
+        #if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        #endif
+        public void Enqueue(SweepEvent node)
+        {
+#if QUEUE_DEBUG
+            if(node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if (Contains(node))
+            {
+                throw new InvalidOperationException("Node is already enqueued: " + node);
+            }
+#endif
+            if (_numNodes >= _nodes.Length - 1)
+            {
+                // double _node array
+                Array.Resize(ref _nodes, _nodes.Length * 2);
+            }
+
+            _numNodes++;
+            _nodes[_numNodes] = node;
+            node.queueIndex = _numNodes;
+            CascadeUp(_nodes[_numNodes]);
+        }
+
+        #if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        #endif
+        private void Swap(SweepEvent node1, SweepEvent node2)
+        {
+            //Swap the nodes
+            _nodes[node1.queueIndex] = node2;
+            _nodes[node2.queueIndex] = node1;
+
+            //Swap their indicies
+            int temp = node1.queueIndex;
+            node1.queueIndex = node2.queueIndex;
+            node2.queueIndex = temp;
+        }
+
+        //Performance appears to be slightly better when this is NOT inlined o_O
+        private void CascadeUp(SweepEvent node)
+        {
+            //aka Heapify-up
+            int parent = node.queueIndex / 2;
+            while(parent >= 1)
+            {
+                SweepEvent parentNode = _nodes[parent];
+                if(HasHigherPriority(parentNode, node))
+                    break;
+
+                //Node has lower priority value, so move it up the heap
+                Swap(node, parentNode); //For some reason, this is faster with Swap() rather than (less..?) individual operations, like in CascadeDown()
+
+                parent = node.queueIndex / 2;
+            }
+        }
+
+        #if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        #endif
+        private void CascadeDown(SweepEvent node)
+        {
+            //aka Heapify-down
+            SweepEvent newParent;
+            int finalQueueIndex = node.queueIndex;
+            while(true)
+            {
+                newParent = node;
+                int childLeftIndex = 2 * finalQueueIndex;
+
+                //Check if the left-child is higher-priority than the current node
+                if(childLeftIndex > _numNodes)
+                {
+                    //This could be placed outside the loop, but then we'd have to check newParent != node twice
+                    node.queueIndex = finalQueueIndex;
+                    _nodes[finalQueueIndex] = node;
+                    break;
+                }
+
+                SweepEvent childLeft = _nodes[childLeftIndex];
+                if(HasHigherPriority(childLeft, newParent))
+                {
+                    newParent = childLeft;
+                }
+
+                //Check if the right-child is higher-priority than either the current node or the left child
+                int childRightIndex = childLeftIndex + 1;
+                if(childRightIndex <= _numNodes)
+                {
+                    SweepEvent childRight = _nodes[childRightIndex];
+                    if(HasHigherPriority(childRight, newParent))
+                    {
+                        newParent = childRight;
+                    }
+                }
+
+                //If either of the children has higher (smaller) priority, swap and continue cascading
+                if(newParent != node)
+                {
+                    //Move new parent to its new index.  node will be moved once, at the end
+                    //Doing it this way is one less assignment operation than calling Swap()
+                    _nodes[finalQueueIndex] = newParent;
+
+                    int temp = newParent.queueIndex;
+                    newParent.queueIndex = finalQueueIndex;
+                    finalQueueIndex = temp;
+                }
+                else
+                {
+                    //See note above
+                    node.queueIndex = finalQueueIndex;
+                    _nodes[finalQueueIndex] = node;
+                    break;
+                }
+            }
+        }
+
+        /// <summary>
+        /// Returns true if 'higher' has higher priority than 'lower', false otherwise.
+        /// Note that calling HasHigherPriority(node, node) (ie. both arguments the same node) will return false
+        /// </summary>
+        #if NET_VERSION_4_5
+        [MethodImpl(MethodImplOptions.AggressiveInlining)]
+        #endif
+        private bool HasHigherPriority(SweepEvent higher, SweepEvent lower)
+        {
+            return higher.CompareTo(lower) < 0;
+        }
+
+        /// <summary>
+        /// Removes the head of the queue and returns it.
+        /// If queue is empty, result is undefined
+        /// O(log n)
+        /// </summary>
+        public SweepEvent Dequeue()
+        {
+#if QUEUE_DEBUG
+            if(_numNodes <= 0)
+            {
+                throw new InvalidOperationException("Cannot call Dequeue() on an empty queue");
+            }
+
+            if(!IsValidQueue())
+            {
+                throw new InvalidOperationException("Queue has been corrupted (Did you update a node priority manually instead of calling UpdatePriority()?" +
+                                                    "Or add the same node to two different queues?)");
+            }
+#endif
+
+            SweepEvent returnMe = _nodes[1];
+            Remove(returnMe);
+            return returnMe;
+        }
+
+        /// <summary>
+        /// Resize the queue so it can accept more nodes.  All currently enqueued nodes are remain.
+        /// Attempting to decrease the queue size to a size too small to hold the existing nodes results in undefined behavior
+        /// O(n)
+        /// </summary>
+        public void Resize(int maxNodes)
+        {
+#if QUEUE_DEBUG
+            if (maxNodes <= 0)
+            {
+                throw new InvalidOperationException("Queue size cannot be smaller than 1");
+            }
+
+            if (maxNodes < _numNodes)
+            {
+                throw new InvalidOperationException("Called Resize(" + maxNodes + "), but current queue contains " + _numNodes + " nodes");
+            }
+#endif
+
+            SweepEvent[] newArray = new SweepEvent[maxNodes + 1];
+            int highestIndexToCopy = Math.Min(maxNodes, _numNodes);
+            for (int i = 1; i <= highestIndexToCopy; i++)
+            {
+                newArray[i] = _nodes[i];
+            }
+            _nodes = newArray;
+        }
+
+        /// <summary>
+        /// Returns the head of the queue, without removing it (use Dequeue() for that).
+        /// If the queue is empty, behavior is undefined.
+        /// O(1)
+        /// </summary>
+        public SweepEvent First
+        {
+            get
+            {
+#if QUEUE_DEBUG
+                if(_numNodes <= 0)
+                {
+                    throw new InvalidOperationException("Cannot call .First on an empty queue");
+                }
+#endif
+
+                return _nodes[1];
+            }
+        }
+
+        private void OnNodeUpdated(SweepEvent node)
+        {
+            //Bubble the updated node up or down as appropriate
+            int parentIndex = node.queueIndex / 2;
+            SweepEvent parentNode = _nodes[parentIndex];
+
+            if(parentIndex > 0 && HasHigherPriority(node, parentNode))
+            {
+                CascadeUp(node);
+            }
+            else
+            {
+                //Note that CascadeDown will be called if parentNode == node (that is, node is the root)
+                CascadeDown(node);
+            }
+        }
+
+        /// <summary>
+        /// Removes a node from the queue.  The node does not need to be the head of the queue.  
+        /// If the node is not in the queue, the result is undefined.  If unsure, check Contains() first
+        /// O(log n)
+        /// </summary>
+        public void Remove(SweepEvent node)
+        {
+#if QUEUE_DEBUG
+            if(node == null)
+            {
+                throw new ArgumentNullException("node");
+            }
+            if(!Contains(node))
+            {
+                throw new InvalidOperationException("Cannot call Remove() on a node which is not enqueued: " + node);
+            }
+#endif
+
+            //If the node is already the last node, we can remove it immediately
+            if (node.queueIndex == _numNodes)
+            {
+                _nodes[_numNodes] = null;
+                _numNodes--;
+                return;
+            }
+
+            //Swap the node with the last node
+            SweepEvent formerLastNode = _nodes[_numNodes];
+            Swap(node, formerLastNode);
+            _nodes[_numNodes] = null;
+            _numNodes--;
+
+            //Now bubble formerLastNode (which is no longer the last node) up or down as appropriate
+            OnNodeUpdated(formerLastNode);
+        }
+
+        public IEnumerator<SweepEvent> GetEnumerator()
+        {
+            for(int i = 1; i <= _numNodes; i++)
+                yield return _nodes[i];
+        }
+
+        /// <summary>
+        /// <b>Should not be called in production code.</b>
+        /// Checks to make sure the queue is still in a valid state.  Used for testing/debugging the queue.
+        /// </summary>
+        public bool IsValidQueue()
+        {
+            for(int i = 1; i < _nodes.Length; i++)
+            {
+                if(_nodes[i] != null)
+                {
+                    int childLeftIndex = 2 * i;
+                    if(childLeftIndex < _nodes.Length && _nodes[childLeftIndex] != null && HasHigherPriority(_nodes[childLeftIndex], _nodes[i]))
+                        return false;
+
+                    int childRightIndex = childLeftIndex + 1;
+                    if(childRightIndex < _nodes.Length && _nodes[childRightIndex] != null && HasHigherPriority(_nodes[childRightIndex], _nodes[i]))
+                        return false;
+                }
+            }
+            return true;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/High-Speed-Priority-Queue/SweepEventPriorityQueue.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: db62ab1d9d5098f47ab5393ae603107a
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/IVelocityProvider.cs

@@ -0,0 +1,12 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    public interface IVelocityProvider
+    {
+        /// <summary>
+        /// Current velocity relative to the world.
+        /// </summary>
+        Vector2 WorldVelocity { get; }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/IVelocityProvider.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: d3b2c212f6466574c8c5641690193a96
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 32a41e6786a58fc4b8936609634bae30
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgent.cs

@@ -0,0 +1,954 @@
+using UnityEngine;
+using System;
+using System.Collections.Generic;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Represents a pathfinding entity.
+    /// </summary>
+    /// <remarks>
+    /// This components handles the interaction with the asynchronous pathfinding system.
+    /// It assumes the agent is a point located at <c>transform.position</c>. 
+    /// Automatic movement will directly modify the transform this script is attached to. See \ref navagent_movement "NavAgent's build-in movement" for more detail on movement.
+    /// ## States
+    /// At heart the NavAgent is a state machine with the following states:
+    /// <list type="bullet">
+    ///     <item>
+    ///         <c>Idle</c>\n
+    ///         <description>
+    ///         In this state the agent does nothing and is ready to path to a new location.
+    ///         </description>
+    ///     </item>
+    ///     <item>
+    ///         <c>Planning</c>\n
+    ///         <description>
+    ///         The agent has made a <see cref="PathRequest">path request</see> and is now waiting for its result.
+    ///         A call to <see cref="PathTo"/> for example would make the agent switch to this state.
+    ///         If the path calculation succeeded, the agent switches into the <c>FollowPath</c> state.
+    ///         If it didn't succeed however, the agent will switch back into the <c>Idle</c> state.
+    ///         </description>
+    ///     </item>
+    ///     <item>
+    ///         <c>FollowPath</c>\n
+    ///         <description>
+    ///         The agent will follow the a previously calculated path. Depending on whether <see cref="autoSegmentMovement"/> or <see cref="autoLinkMovement"/> is set,
+    ///         the path will be followed automatically. The agent has build-in ways to traverse the build-in link types. They don't make use of the physics system.
+    ///         The path will be recalculated at a set interval determined by <see cref="autoRepathIntervall"/>. This is to ensure the path is up to date with changes in the world.
+    ///         No reactions to changes in the world between recalculations is possible.
+    ///
+    ///         Following a path is further subdivided into three states:
+    ///         <list>
+    ///             <item>
+    ///                <c>Segment movement</c>\n
+    ///                <description>
+    ///                The agent moves on a line segment.
+    ///                If you move the agent manually, call <see cref="CompleteSegmentTraversal"/> to switch to the next state.
+    ///                </description>
+    ///             </item>
+    ///             <item>
+    ///                <c>Wait for link</c>\n
+    ///                <description>
+    ///                This state is only entered if after the agent finshes moving on a segment, the link it wants to take is currently not traversable.
+    ///                The agent will wait for the link to become traversable again.
+    ///                </description>
+    ///             </item>
+    ///             <item>
+    ///                <c>Traverse link</c>\n
+    ///                <description>
+    ///                The agent will move on the link.
+    ///                If you move the agent manually, call <see cref="CompleteLinkTraversal"/> to begin traversing the next segment.
+    ///                </description>
+    ///             </item>
+    ///         </list>
+    ///         </description>
+    ///     </item>
+    ///     <item>
+    ///         <c>LostPath</c>\n
+    ///         <description>
+    ///         The agent was previously in the <c>FollowPath</c> state, but <see cref="LostPath"/> was called.
+    ///         In this state, the agent will periodically attempt to find a path to its last goal.
+    ///         This is useful, for if the agent unexpectedly was moved of its previously followed path, it can still attempt to reach its goal. 
+    ///         This state is only entered, when in state <c>FollowPath</c> and <see cref="LostPath"/> is called. 
+    ///         </description>
+    ///     </item>
+    /// </list>
+    /// ## Pathfinding properties
+    /// A NavAgent has a few properties relevant to the pathfinder.
+    /// <list>
+    ///     <item>
+    ///         <see cref="height"/>\n
+    ///         <description>
+    ///         Only segments and links with enough free space are considered.
+    ///         </description>
+    ///     </item>
+    ///     <item>
+    ///         <see cref="maxSlopeAngle"/>\n
+    ///         <description>
+    ///         Only segments that don't exceed this angle are considered traversable.
+    ///         0° = ground, 90° = straight walls and 180° = ceiling.
+    ///         </description>
+    ///     </item>
+    ///     <item>
+    ///         <see cref="linkTraversalCostMultipliers"/>\n
+    ///         <description>
+    ///         For each link one multiplier from this array is applied to its traversal costs.
+    ///         With multiplier values <= 0 you can completely exclude certain link types from traversal.
+    ///         </description>
+    ///     </item>
+    ///     <item>
+    ///         <see cref="navTagTraversalCostMultipliers"/>\n
+    ///         <description>
+    ///         Links and parts of segments can be tagged with a NavTag. NavTag with index 0 is considered the default and applied to all segments
+    ///         that don't have another NavTag.
+    ///         As with <see cref="linkTraversalCostMultipliers"/> multiplier values <= 0 completely exclude NavTags from traversal.
+    ///         </description>
+    ///     </item>
+    /// </list>
+    /// </remarks>
+    [AddComponentMenu("PathBerserker2d/Nav Agent")]
+    [ScriptExecutionOrder(-50)]
+    public class NavAgent : MonoBehaviour
+    {
+        public enum State
+        {
+            /// Agent is doing nothing.
+            Idle,
+            /// Agent is following a path.
+            FollowPath,
+        }
+
+        private enum MovementState
+        {
+            OnSegment,
+            OnLink,
+            WaitForLinkOnSegment,
+        }
+
+        public State CurrentStatus => status;
+        public bool IsIdle => status == State.Idle && currentPathRequest?.Status == PathRequest.RequestState.Draft;
+        public bool IsFollowingAPath => status == State.FollowPath;
+
+        public float Height => height;
+        public float MaxSlopeAngle => maxSlopeAngle;
+        public bool IsOnLink => IsFollowingAPath && movementState == MovementState.OnLink;
+        public bool IsMovingOnSegment => IsFollowingAPath && movementState == MovementState.OnSegment;
+        public bool IsWaitingForLink => IsFollowingAPath && movementState == MovementState.WaitForLinkOnSegment;
+
+        /// <summary>
+        /// If true, either IsMovingOnSegment is true, or the agent is waiting to traverse an untraversable link.
+        /// </summary>
+        public bool IsOnSegment => IsFollowingAPath && movementState != MovementState.OnLink;
+
+        /// <summary>
+        /// Check, if the current mapped position of the agent is valid. The mapped position can only be valid, if the agent is close to the ground. (Close, not necessarily directly on the ground)
+        /// </summary>
+        public bool HasValidPosition => !currentMappedPosition.IsInvalid();
+
+        /// <summary>
+        /// True, if Stop() was called and agent hasn't yet stopped
+        /// </summary>
+        public bool IsStopping => stopRequested;
+
+        /// <summary>
+        /// True, if the agent is on the last segment of its path. 
+        /// </summary>
+        public bool IsOnGoalSegment => IsOnSegment && !Path.HasNext;
+
+        /// <summary>
+        /// Link of the path segment the agent is on, or null.
+        /// </summary>
+        [Obsolete("Use CurrentPathSegment.link")]
+        public INavLinkInstance CurrentLink => currentPath?.Current.link ?? null;
+
+        /// <summary>
+        /// Link type of the path segment the agent is on, or ""
+        /// </summary>
+        [Obsolete("Use CurrentPathSegment.link.LinkTypeName")]
+        public string CurrentLinkType => currentPath?.Current.link?.LinkTypeName ?? "";
+
+        /// <summary>
+        /// Link start of the path segment the agent is on, or Vector2.zero. Can change each frame, if the link start is on a moving platform.
+        /// </summary>
+        [Obsolete("Use CurrentPathSegment.LinkStart")]
+        public Vector2 CurrentLinkStart => currentPath?.Current.LinkStart ?? Vector2.zero;
+
+        /// <summary>
+        /// Segment normal of the path segment the agent is or normal of the currently mapped position, or Vector2.up
+        /// </summary>
+        public Vector2 CurrentSegmentNormal => IsFollowingAPath ? currentPath.Current.Normal : (currentMappedPosition.IsValid() ? currentMappedPosition.Normal : Vector2.up);
+
+        /// <summary>
+        /// Current path segment if agent follows a path or null.
+        /// </summary>
+        public PathSegment CurrentPathSegment => currentPath?.Current;
+
+        /// <summary>
+        /// Segment normal of the next segment on the path, or Vector2.zero
+        /// </summary>
+        [Obsolete("Use CurrentPathSegment.Next.Normal")]
+        public Vector2 NextSegmentNormal => currentPath?.NextSegment?.Normal ?? Vector2.zero;
+
+        /// <summary>
+        /// Current subgoal the agent is moving towards. May either be a link start or a link end. If it lies on a moving platform, the value may change from frame to frame.
+        /// </summary>
+        public Vector2 PathSubGoal
+        {
+            get
+            {
+                if (IsFollowingAPath)
+                {
+                    if (movementState == MovementState.OnLink)
+                        return currentPath.Current.LinkEnd;
+                    else
+                        return currentPath.Current.LinkStart;
+                }
+                return Vector2.zero;
+            }
+        }
+        /// <summary>
+        /// Overall goal of the path the agent is on
+        /// </summary>
+        public Vector2? PathGoal => currentPath?.Goal;
+
+        /// <summary>
+        /// Shorthand for transform.position
+        /// </summary>
+        public Vector2 Position
+        {
+            get => transform.position;
+            set => transform.position = new Vector3(value.x, value.y, transform.position.z);
+        }
+
+        /// <summary>
+        /// Combination of all NavTags at the position of the agent.
+        /// </summary>
+        public int CurrentNavTagVector => IsFollowingAPath ? currentPath.Current.GetTagVector(Position) : (currentMappedPosition.cluster?.GetNavTagVector(currentMappedPosition.t) ?? 0);
+
+        /// <summary>
+        /// Time agent spend on link. Does not include waiting for link to become traversable.
+        /// </summary>
+        public float TimeOnLink { get; private set; }
+
+
+        public delegate void FailedToFindPathDelegate(NavAgent agent);
+
+        /// <summary>
+        /// Fired when agent begins moving on a link.
+        /// </summary>
+        public event Action<NavAgent> OnStartLinkTraversal;
+
+        /// <summary>
+        /// Fired when agent is moving on a link.
+        /// </summary>
+        public event Action<NavAgent> OnLinkTraversal;
+
+        /// <summary>
+        /// Fired when agent start moving on a segment.
+        /// </summary>
+        public event Action<NavAgent> OnStartSegmentTraversal;
+
+        /// <summary>
+        /// Fired when agent is moving on a segment.
+        /// </summary>
+        public event Action<NavAgent> OnSegmentTraversal;
+
+        /// <summary>
+        /// Fired when the agent fails to find a path
+        /// </summary>
+        public event Action<NavAgent> OnFailedToFindPath;
+
+        /// <summary>
+        /// Fired when agent stops after Stop() or ForceStop() was called. For ForceStop() this happens instantly. For Stop() this happens after the agent stopped.
+        /// </summary>
+        public event Action<NavAgent> OnStop;
+
+        /// <summary>
+        /// Fired when agent reaches its current goal.
+        /// </summary>
+        public event Action<NavAgent> OnReachedGoal;
+
+        /// <summary>
+        /// Called when the agent starts following a new path.
+        /// NOTE: Also called, after successfully recalculating a path, even if the path itself does not change.
+        /// </summary>
+        public event Action<NavAgent> OnStartFollowingNewPath;
+
+        internal Path Path => currentPath;
+        internal int NavTagMask => navTagMask;
+
+
+
+        [Header("Pathplanning")]
+        [SerializeField]
+        // protect! should not be changeable, unless not making a path request
+        private float height = 1;
+        [SerializeField]
+        // protect! should not be changeable, unless not making a path request
+        // 90 = doesnt matter
+        [Range(0, 180)]
+        [Tooltip("Maximum slope the agent can walk on. 180 = unlimited")]
+        private float maxSlopeAngle = 180;
+
+        /// <summary>
+        /// Delay in seconds between recalculations of current path. This enables the agent to react to changes in the world. Higher values are better for performance.
+        /// </summary>
+        [Tooltip("Interval at which an agent will recalculate its current path to react to world changes in seconds. Higher value improves performance.")]
+        [SerializeField]
+        float autoRepathIntervall = 1f;
+        [Tooltip("Maximum distance an agent can be from the start of a calculated path, to start following it. If the distance is to large, the path is thrown out.")]
+        [SerializeField]
+        float maximumDistanceToPathStart = 0.7f;
+
+        [SerializeField]
+        float[] linkTraversalCostMultipliers;
+
+        /// <summary>
+        /// If true and no path exists between start and goal, the NavAgent will try to find a path to the closest reachable position instead. Does not work with multiple targets!
+        /// </summary>
+        [Tooltip("If true and no path exists between start and goal, will try to find a path to the closest reachable position instead. Does not work with multiple targets!")]
+        [SerializeField]
+        bool allowCloseEnoughPath = false;
+
+        [Obsolete("Moved to out and into a separate component. Only for migration purposes.", true)]
+        [Tooltip("Speed on segments in unit/s.")]
+        [SerializeField]
+        public float movementSpeed = 5;
+
+
+        [Obsolete("Moved to out and into a separate component. Only for migration purposes.", true)]
+        [Tooltip("Speed on corner links in degrees/s.")]
+        [SerializeField]
+        public float cornerSpeed = 100;
+
+        [Obsolete("Moved to out and into a separate component. Only for migration purposes.", true)]
+        [Tooltip("Speed on jump links in unit/s.")]
+        [SerializeField]
+        public float jumpSpeed = 5;
+
+        [Obsolete("Moved to out and into a separate component. Only for migration purposes.", true)]
+        [Tooltip("Speed on fall links in unit/s.")]
+        [SerializeField]
+        public float fallSpeed = 5;
+
+        [Obsolete("Moved to out and into a separate component. Only for migration purposes.", true)]
+        [Tooltip("Speed on climb links in unit/s.")]
+        [SerializeField]
+        public float climbSpeed = 5;
+
+        [Tooltip("If true, will print debug messages.")]
+        [SerializeField]
+        public bool enableDebugMessages = false;
+
+
+        /// <summary>
+        /// Traversal cost multipliers for nav tags. A value less or equal to 0 prohibits the agent from traversing that tag.
+        /// </summary>
+        [SerializeField]
+        float[] navTagTraversalCostMultipliers;
+
+
+        [SerializeField, ReadOnly]
+        private State status;
+
+        [SerializeField, HideInInspector]
+        private int navTagMask;
+
+        internal NavSegmentPositionPointer currentMappedPosition;
+        internal PathRequest currentPathRequest;
+
+        private Path currentPath = null;
+        private PathRequest repathPathRequest;
+        private float lastRepathTime;
+        private MovementState movementState;
+        private bool traversedLinkSinceLastRepath;
+        private bool traversedLinkSinceLastPath;
+        private bool stopRequested;
+
+        #region UNITY_METHODS
+        private void OnEnable()
+        {
+            status = State.Idle;
+            currentPathRequest = new PathRequest(this);
+            repathPathRequest = new PathRequest(this);
+        }
+
+        private void Start()
+        {
+            UpdateMappedPosition();
+        }
+
+        private void OnValidate()
+        {
+            if (linkTraversalCostMultipliers == null)
+                linkTraversalCostMultipliers = new float[0];
+            if (navTagTraversalCostMultipliers == null)
+                navTagTraversalCostMultipliers = new float[0];
+
+            if (linkTraversalCostMultipliers.Length != PathBerserker2dSettings.NavLinkTypeNames.Length)
+            {
+                Utility.ResizeWithDefault(ref linkTraversalCostMultipliers, PathBerserker2dSettings.NavLinkTypeNames.Length, 1);
+            }
+            if (navTagTraversalCostMultipliers.Length != PathBerserker2dSettings.NavTags.Length)
+            {
+                Utility.ResizeWithDefault(ref navTagTraversalCostMultipliers, PathBerserker2dSettings.NavTags.Length, 1);
+            }
+
+            navTagMask = GetNavTagMask();
+        }
+
+        private void Update()
+        {
+            UpdateMappedPosition();
+            HandlePathRequest();
+
+            switch (status)
+            {
+                case State.FollowPath:
+                    Repath();
+
+                    if (movementState == MovementState.OnLink)
+                    {
+                        //check if link still exists
+                        if (CurrentPathSegment.link == null)
+                        {
+                            // link was destroyed. Wait for repath
+                            break;
+                        }
+
+                        TimeOnLink += Time.deltaTime;
+                        OnLinkTraversal?.Invoke(this);
+                    }
+                    else if (movementState == MovementState.OnSegment)
+                    {
+                        if (stopRequested)
+                        {
+                            status = State.Idle;
+                            stopRequested = false;
+                            OnStop?.Invoke(this);
+                        }
+                        else
+                        {
+                            OnSegmentTraversal?.Invoke(this);
+                        }
+                    }
+                    else
+                    {
+                        if (currentPath.Current.link.IsTraversable)
+                        {
+                            StartTraversingLink();
+                        }
+                    }
+                    break;
+            }
+        }
+        #endregion
+
+        /// <summary>
+        /// Repositions the agent at the nearest segment the agent could be standing at. Segments the agent could not be at do to its tag or slope will be ignored.
+        /// </summary>
+        /// <returns>True, if warping was successful</returns>
+        public bool WarpToNearestSegment(float maximumWarpDistance = 10)
+        {
+            if (!currentMappedPosition.IsInvalid())
+            {
+                // already close enough
+                this.Position = currentMappedPosition.Position;
+                return true;
+            }
+
+            NavSegmentPositionPointer p;
+            if (PBWorld.TryMapPoint(Position, maximumWarpDistance, this, out p))
+            {
+                this.Position = p.Position;
+                return true;
+            }
+            return false;
+        }
+
+        /// <summary>
+        /// Starts the process of pathfinding to the closest of the given goals.
+        /// NOTE: Do not call this method every frame. Calculating a path takes longer than a frame, so the agent will never start moving.
+        /// </summary>
+        /// <seealso cref="UpdatePath(Vector2[])"/>
+        /// <param name="goals">Goals to pathfind to.</param>
+        /// <returns>True, if the at least 1 goal and the agents own position could be mapped. This does not mean, that a path towards a goal exists.</returns>
+        public bool PathTo(params Vector2[] goals)
+        {
+            Stop();
+            return UpdatePath(goals);
+        }
+
+        /// <summary>
+        /// Starts the process of pathfinding to the given goal.
+        /// NOTE: Do not call this method every frame. Calculating a path takes longer than a frame, so the agent will never start moving.
+        /// </summary>
+        /// <seealso cref="UpdatePath(Vector2)"/>
+        /// <param name="goals">Goals to pathfind to.</param>
+        /// <returns>True, if the at least 1 goal and the agents own position could be mapped. This does not mean, that a path towards a goal exists.</returns>
+        public bool PathTo(Vector2 goal)
+        {
+            Stop();
+            return UpdatePath(goal);
+        }
+
+        /// <summary>
+        /// Starts the process of pathfinding to the closest given goal.
+        /// NOTE: Do not call this method every frame. Calculating a path takes longer than a frame, so the agent will never start moving.
+        /// </summary>
+        /// <returns>True, the agents own position could be mapped. This does not mean, that a path towards the goal exists.</returns>
+        private bool PathTo(IList<NavSegmentPositionPointer> goalPs)
+        {
+            Stop();
+            return UpdatePath(goalPs);
+        }
+
+        /// <summary>
+        /// Starts the process of pathfinding to the closest of the given goals. Will continue moving until the calculations for the new path are completed.
+        /// NOTE: Do not call this method every frame. Calculating a path takes longer than a frame, so the agent will never start moving.
+        /// </summary>
+        /// <seealso cref="PathTo(Vector2[])"/>
+        /// <param name="goals">Goals to pathfind to.</param>
+        /// <returns>True, if the at least 1 goal and the agents own position could be mapped. This does not mean, that a path towards a goal exists.</returns>
+        public bool UpdatePath(params Vector2[] goals)
+        {
+            if (currentMappedPosition.IsInvalid())
+                return false;
+
+            List<NavSegmentPositionPointer> goalPs = new List<NavSegmentPositionPointer>(goals.Length);
+            NavSegmentPositionPointer p;
+            for (int i = 0; i < goals.Length; i++)
+            {
+                float maxDist = Vector2.Distance(Position, goals[i]) + 0.1f;
+                if (PBWorld.TryMapPoint(goals[i], maxDist, out p) && (allowCloseEnoughPath || CouldBeLocatedAt(p)))
+                {
+                    goalPs.Add(p);
+                }
+            }
+            return UpdatePath(goalPs);
+        }
+
+        /// <summary>
+        /// Starts the process of pathfinding to the given goal. Will continue moving until the calculations for the new path are completed.
+        /// NOTE: Do not call this method every frame. Calculating a path takes longer than a frame, so the agent will never start moving.
+        /// </summary>
+        /// <seealso cref="PathTo(Vector2)"/>
+        /// <param name="goals">Goals to pathfind to.</param>
+        /// <returns>True, if the at least 1 goal and the agents own position could be mapped. This does not mean, that a path towards a goal exists.</returns>
+        public bool UpdatePath(Vector2 goal)
+        {
+            if (currentMappedPosition.IsInvalid())
+                return false;
+
+            float maxDist = Vector2.Distance(Position, goal) + 0.1f;
+            NavSegmentPositionPointer p;
+            if (!PBWorld.TryMapPoint(goal, maxDist, out p) || (!allowCloseEnoughPath && !CouldBeLocatedAt(p)))
+                return false;
+
+            return UpdatePath(new NavSegmentPositionPointer[] { p });
+        }
+
+        /// <summary>
+        /// Simple distance check between agent and CurrentSubGoal.
+        /// </summary>
+        /// <returns>True, if distance is less than maxDist</returns>
+        public bool HasReachedCurrentSubGoal(float maxDist = 0.05f)
+        {
+            Vector2 delta = PathSubGoal - Position;
+            float distance = delta.magnitude;
+            return distance < maxDist;
+        }
+
+        /// <summary>
+        /// Starts the process of pathfinding to the closest given goal. Will continue moving until the calculations for the new path are completed.
+        /// </summary>
+        /// <returns>True, the agents own position could be mapped. This does not mean, that a path towards the goal exists.</returns>
+        private bool UpdatePath(IList<NavSegmentPositionPointer> goalPs)
+        {
+            if (currentPathRequest.Status == PathRequest.RequestState.Pending)
+                return false;
+
+            if (goalPs.Count == 0)
+                return false;
+
+            if (currentMappedPosition.IsInvalid())
+                return false;
+
+            currentPathRequest.start = currentMappedPosition;
+            currentPathRequest.goals = goalPs;
+            PBWorld.PathTo(currentPathRequest);
+            traversedLinkSinceLastPath = false;
+            return true;
+        }
+
+        /// <summary>
+        /// Start pathfinding to a random position on the NavGraph. It cannot grantee that this position is reachable. Does the agent might not move after this is called.
+        /// </summary>
+        /// <returns></returns>
+        public bool SetRandomDestination()
+        {
+            if (currentMappedPosition.IsInvalid())
+                return false;
+
+            Vector2 goal = PBWorld.GetRandomPointOnGraph();
+            return PathTo(goal);
+        }
+
+
+        /// <summary>
+        /// If you implement link traversal yourself, call this to complete a link traversal.
+        /// </summary>
+        public void CompleteLinkTraversal()
+        {
+            if (IsOnLink)
+            {
+                currentPath.MoveNext();
+                StartTraversingSegment();
+            }
+        }
+
+        /// <summary>
+        /// If you implement segment traversal yourself, call this to complete a segment traversal.
+        /// </summary>
+        public void CompleteSegmentTraversal()
+        {
+            if (movementState == MovementState.OnSegment)
+            {
+                if (currentPath.HasNext)
+                {
+                    if (CurrentPathSegment.link.IsTraversable)
+                    {
+                        StartTraversingLink();
+                    }
+                    else
+                    {
+                        movementState = MovementState.WaitForLinkOnSegment;
+                    }
+                }
+                else
+                {
+                    status = State.Idle;
+                    OnReachedGoal?.Invoke(this);
+                }
+            }
+        }
+
+        /// <summary>
+        /// Determines, if in this agent is allowed to traverse the given link.
+        /// </summary>
+        public bool CanTraverseLink(INavLinkInstance link)
+        {
+            int linkType = link.LinkType;
+            return linkType == -1 || (GetLinkTraversalMultiplier(linkType) > 0 && height <= link.Clearance);
+        }
+
+        /// <summary>
+        /// Get the traversal cost multiplier for a given link type.
+        /// </summary>
+        public float GetLinkTraversalMultiplier(int linkType)
+        {
+            return linkTraversalCostMultipliers[linkType];
+        }
+
+        /// <summary>
+        /// Get the traversal cost multiplier for a given nav tag.
+        /// </summary>
+        public float GetNavTagTraversalMultiplier(int navTag)
+        {
+            return navTagTraversalCostMultipliers[navTag] <= 0 ? float.PositiveInfinity : navTagTraversalCostMultipliers[navTag];
+        }
+
+        /// <summary>
+        /// Whether the agents current position contains the given NavTag. NOTE: Does not work, if the agent is not currently moving on a path.
+        /// </summary>
+        /// <returns>True, if current position has supplied NavTag.</returns>
+        public bool IsOnSegmentWithTag(int navTag)
+        {
+            if (IsOnSegment)
+                return (CurrentNavTagVector & (1 << navTag)) != 0;
+            else
+                return false;
+        }
+
+        /// <summary>
+        /// Stops the current path following at the first opportunity. Link traversal will be completed before the agent stops.
+        /// </summary>
+        public void Stop()
+        {
+            stopRequested = true;
+            if (currentPathRequest.Status == PathRequest.RequestState.Pending)
+                currentPathRequest = new PathRequest(this);
+        }
+
+        /// <summary>
+        /// Stops the current path following instantly. Agent might stop wihle traversing a link (e.g. while jumping in mid air)
+        /// </summary>
+        public void ForceStop()
+        {
+            status = State.Idle;
+            currentPathRequest = new PathRequest(this);
+            OnStop?.Invoke(this);
+        }
+
+        /// <summary>
+        /// Tries to map the "other" and checks if the agent is mapped to the same segment. 
+        /// If "other" can't be mapped this will return null. 
+        /// Agents on a link will always return false.
+        /// If this agent currently can't be mapped this will return null.
+        /// </summary>
+        /// <param name="other"></param>
+        public bool? IsOnSameSegmentAs(Vector2 other)
+        {
+            if (IsOnLink)
+                return false;
+
+            NavSegmentPositionPointer p;
+            if (!PBWorld.TryMapPoint(other, out p) || currentMappedPosition.IsInvalid())
+                return null;
+            return currentMappedPosition.surface == p.surface && currentMappedPosition.cluster == p.cluster;
+        }
+
+        /// <summary>
+        /// Enumerates the points on the currently followed path. Corner links will result in the same point being enumerated twice in a row. First point will be the agents current position.
+        /// </summary>
+        public IEnumerable<Vector2> PathPoints()
+        {
+            if (!IsFollowingAPath)
+                yield break;
+
+            yield return Position;
+
+            var seg = currentPath.Current;
+            if (IsOnSegment)
+                yield return seg.LinkStart;
+
+            if (seg.Next != null)
+            {
+                yield return seg.LinkEnd;
+
+                seg = seg.Next;
+                while (seg.Next != null)
+                {
+                    yield return seg.LinkStart;
+                    yield return seg.LinkEnd;
+                    seg = seg.Next;
+                }
+
+                yield return seg.LinkStart;
+            }
+        }
+
+
+
+        /// <summary>
+        /// Creates a pathrequest for this agent using the specified start and goal. The PathRequest is for your own use. The agent will take no further action. Use it to plan theoretical paths, without the agent moving. See also PBWorld.PathTo()
+        /// </summary>
+        /// <returns>A PathRequest or null, if start or goal couldn't be mapped.</returns>
+        public PathRequest CreatePathRequest(Vector2 start, Vector2 goal)
+        {
+            float maxDist = Vector2.Distance(Position, goal) + 0.1f;
+            NavSegmentPositionPointer startPointer;
+            if (!PBWorld.TryMapPoint(start, maxDist, this, out startPointer))
+                return null;
+
+            NavSegmentPositionPointer goalPointer;
+            if (!PBWorld.TryMapPoint(goal, maxDist, out goalPointer) || (!allowCloseEnoughPath && !CouldBeLocatedAt(goalPointer)))
+                return null;
+
+            PathRequest request = new PathRequest(this);
+            request.start = startPointer;
+            request.goals = new[] { goalPointer };
+
+            return request;
+        }
+
+        /// <summary>
+        /// Convenience function that will return if the agent could reach the given point from it's current location. It runs synchronously which is not optimal for performance.
+        /// </summary>
+        public bool CanReach(Vector2 goal)
+        {
+            var pr = CreatePathRequest(Position, goal);
+            PBWorld.PathTo(pr);
+
+            while (pr.Status != PathRequest.RequestState.Finished && pr.Status != PathRequest.RequestState.Failed)
+            {
+                // fast spinning
+            }
+            return pr.Status == PathRequest.RequestState.Finished;
+        }
+
+        private int GetNavTagMask()
+        {
+            int navTagMask = 0;
+            for (int i = 0; i < navTagTraversalCostMultipliers.Length; i++)
+            {
+                if (navTagTraversalCostMultipliers[i] <= 0)
+                    navTagMask |= 1 << i;
+            }
+            return ~navTagMask;
+        }
+
+        internal bool CanTraverseSegment(Vector2 segNormal, float minClearance)
+        {
+            return Vector2.Angle(Vector2.up, segNormal) <= maxSlopeAngle && minClearance >= height;
+        }
+
+        internal bool CouldBeLocatedAt(NavSegmentPositionPointer positionPointer)
+        {
+            return Vector2.Angle(Vector2.up, positionPointer.Normal) <= maxSlopeAngle && positionPointer.cluster.GetClearanceAlongSegment(positionPointer.t) >= height && (positionPointer.cluster.GetNavTagVector(positionPointer.t) & ~navTagMask) == 0;
+        }
+
+        private void StartTraversingLink()
+        {
+            movementState = MovementState.OnLink;
+            traversedLinkSinceLastRepath = true;
+            traversedLinkSinceLastPath = true;
+            TimeOnLink = 0;
+            OnStartLinkTraversal?.Invoke(this);
+        }
+
+        private void StartTraversingSegment()
+        {
+            movementState = MovementState.OnSegment;
+            OnStartSegmentTraversal?.Invoke(this);
+        }
+
+        private void UpdateMappedPosition()
+        {
+            // probably not on ground when on link
+            if (IsOnLink)
+            {
+                // make sure to set the path to invalid
+                currentMappedPosition = NavSegmentPositionPointer.Invalid;
+                return;
+            }
+            //if (Time.time >= timeToRemapPosition)
+            //{
+            // timeToRemapPosition = Time.time + 0.2f + UnityEngine.Random.value * 0.1f;
+
+            PBWorld.TryMapAgent(Position, currentMappedPosition, this, out currentMappedPosition);
+
+            // edge case fix
+            // happens if the navagent is on a corner and mapping disagrees with path
+            if (IsFollowingAPath && currentMappedPosition.cluster != currentPath.Current.cluster)
+            {
+                if (currentMappedPosition.t <= 0.05f)
+                {
+                    currentMappedPosition = new NavSegmentPositionPointer(currentMappedPosition.surface, currentPath.Current.cluster, 0);
+                }
+                else if (currentMappedPosition.t >= currentMappedPosition.cluster.Length - 0.05f)
+                {
+                    currentMappedPosition = new NavSegmentPositionPointer(currentMappedPosition.surface, currentPath.Current.cluster, currentPath.Current.cluster.Length);
+                }
+            }
+        }
+
+        private void Repath()
+        {
+            switch (repathPathRequest.Status)
+            {
+                case PathRequest.RequestState.Draft:
+                    if (IsOnSegment && !currentMappedPosition.IsInvalid() && Time.time - lastRepathTime >= Mathf.Max(0.1f, autoRepathIntervall))
+                    {
+                        repathPathRequest.start = currentMappedPosition;
+                        repathPathRequest.goals = currentPathRequest.goals;
+
+                        PBWorld.PathTo(repathPathRequest);
+
+                        lastRepathTime = Time.time;
+                        traversedLinkSinceLastRepath = false;
+                    }
+                    break;
+                case PathRequest.RequestState.Failed:
+                    if (repathPathRequest.FailReason == PathRequest.RequestFailReason.NoPathFromStartToGoal
+                        || repathPathRequest.FailReason == PathRequest.RequestFailReason.WorldWasDestroyed)
+                    {
+                        Stop();
+                        OnFailedToFindPath?.Invoke(this);
+                    }
+
+                    repathPathRequest.Reset();
+                    break;
+                case PathRequest.RequestState.Finished:
+                    if (!traversedLinkSinceLastRepath && currentPathRequest.goals == repathPathRequest.goals)
+                    {
+                        StartFollowingPath(repathPathRequest);
+                    }
+
+                    repathPathRequest.Reset();
+                    break;
+            }
+        }
+
+        private void HandlePathRequest()
+        {
+            switch (currentPathRequest.Status)
+            {
+                case PathRequest.RequestState.Failed:
+
+                    if (!allowCloseEnoughPath)
+                    {
+                        Stop();
+
+                        if (currentPathRequest.Status == PathRequest.RequestState.Failed && enableDebugMessages)
+                            Debug.Log($"{name}: Pathrequest failed because: {currentPathRequest.FailReason}");
+
+                        OnFailedToFindPath?.Invoke(this);
+                        currentPathRequest.Reset();
+                    }
+                    else if (currentPathRequest.FailReason == PathRequest.RequestFailReason.NoPathFromStartToGoal && !currentPathRequest.closestReachablePosition.IsInvalid())
+                    {
+                        float maxDistance = 10;
+                        float distance = Vector2.Distance(currentPathRequest.closestReachablePosition.Position, Position);
+                        if (distance < maxDistance)
+                        {
+                            UpdatePath(new List<NavSegmentPositionPointer>() { currentPathRequest.closestReachablePosition });
+                        }
+                        OnFailedToFindPath?.Invoke(this);
+                    }
+                    break;
+                case PathRequest.RequestState.Finished:
+                    if (!traversedLinkSinceLastPath)
+                    {
+                        StartFollowingPath(currentPathRequest);
+                        currentPathRequest.Reset();
+                    }
+                    else
+                    {
+                        traversedLinkSinceLastPath = false;
+                        // retry
+                        if (IsOnSegment && !currentMappedPosition.IsInvalid())
+                            PathTo(currentPathRequest.goals);
+                    }
+                    break;
+            }
+        }
+
+        private bool StartFollowingPath(PathRequest request)
+        {
+#if PBDEBUG
+            Debug.Log("Pathrequest succeed. " + request.Path);
+            Debug.Assert(request.Status == PathRequest.RequestState.Finished);
+#endif
+            // check that we are close to the path start
+            if (Vector2.Distance(request.start.Position, Position) > maximumDistanceToPathStart)
+            {
+#if PBDEBUG
+                Debug.Log("Moved to far away from path start. Not using that path");
+#endif
+                request.Fail(PathRequest.RequestFailReason.ToFarFromStart);
+                return false;
+            }
+
+            stopRequested = false;
+            lastRepathTime = Time.time;
+            this.status = State.FollowPath;
+            currentPath = request.Path;
+
+            StartTraversingSegment();
+
+            OnStartFollowingNewPath?.Invoke(this);
+            return true;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgent.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 3864fd1487d130847b11b82f276d11b6
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: -50
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 19f72ec82365a48439101af4e8b29c4e
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/AdjustRotation.cs

@@ -0,0 +1,32 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Adjust the NavAgents rotation to match the segments rotation.
+    /// </summary>
+    public class AdjustRotation : MonoBehaviour
+    {
+        [SerializeField]
+        public NavAgent agent;
+
+        /// <summary>
+        /// Speed at which the agent is rotated.
+        /// </summary>
+        [SerializeField, Tooltip("Speed at which the agent is rotated.")]
+        public float rotationSpeed = 20;
+
+        private void Update()
+        {
+            if (!agent.IsOnLink || agent.CurrentPathSegment?.link?.LinkTypeName != "corner")
+            {
+                this.transform.rotation = Quaternion.Slerp(transform.rotation, Quaternion.LookRotation(Vector3.forward, this.agent.CurrentSegmentNormal), Time.deltaTime * rotationSpeed);
+            }
+        }
+
+        private void Reset()
+        {
+            agent = GetComponent<NavAgent>();
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/AdjustRotation.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: f33302ff91c92994e9d60e1cf7668b8f
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/Follower.cs

@@ -0,0 +1,93 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Makes a NavAgent follow another.
+    /// </summary>
+    public class Follower : MonoBehaviour
+    {
+        [SerializeField]
+        public NavAgent navAgent = null;
+        [SerializeField]
+        public Transform target = null;
+
+        /// <summary>
+        /// Radius when agent should start moving towards the target. Should be >= travelStopRadius
+        /// </summary>
+        [SerializeField, Tooltip("Radius when agent should start moving towards the target. Should be >= travelStopRadius")]
+        public float closeEnoughRadius = 3;
+
+        /// <summary>
+        /// Radius when agent should stop moving towards the target.
+        /// </summary>
+        [SerializeField, Tooltip("Radius when agent should stop moving towards the target")]
+        public float travelStopRadius = 1;
+
+        /// <summary>
+        /// Using the targets velocity, predicts the targets position in the future and uses this prediction as pathfinding goal. Useful for fast moving enemies. Only works when the target has a Rigidbody2d component or a component that implements IVelocityProvider. (NavAgent does not!)
+        /// </summary>
+        [SerializeField]
+        [Tooltip("Using the targets velocity, predicts the targets position in the future and uses this prediction as pathfinding goal. Useful for fast moving enemies. Only works when the target has a Rigidbody2d component or a component that implements IVelocityProvider. (NavAgent does not!)")]
+        public float targetPredictionTime = 0;
+
+        void Update()
+        {
+            if (target == null)
+                return;
+
+            Vector2 targetPos = GetTargetPosition();
+            float distToTarget = Vector2.Distance(transform.position, targetPos);
+
+            if (distToTarget > closeEnoughRadius && 
+                !(navAgent.PathGoal.HasValue &&  Vector2.Distance(navAgent.PathGoal.Value, targetPos) < travelStopRadius))
+            {
+                if (!navAgent.UpdatePath(targetPos) && targetPredictionTime > 0)
+                {
+                    navAgent.UpdatePath(target.position);
+                }
+            }
+            else if (distToTarget < travelStopRadius)
+            {
+                navAgent.Stop();
+            }
+        }
+
+        private void OnDrawGizmosSelected()
+        {
+            Gizmos.color = Color.green;
+            Gizmos.DrawWireSphere(transform.position, closeEnoughRadius);
+
+            Gizmos.color = Color.blue;
+            Gizmos.DrawWireSphere(transform.position, travelStopRadius);
+        }
+
+        private void OnValidate()
+        {
+            closeEnoughRadius = Mathf.Max(travelStopRadius, closeEnoughRadius);
+        }
+
+        private void Reset()
+        {
+            navAgent = GetComponent<NavAgent>();
+        }
+
+        private Vector2 GetTargetPosition()
+        {
+            Vector2 tpos = target.position;
+            if (targetPredictionTime > 0)
+            {
+                IVelocityProvider velocityProvider = target.GetComponent<IVelocityProvider>();
+                if (velocityProvider != null)
+                    return tpos + velocityProvider.WorldVelocity * targetPredictionTime;
+
+                Rigidbody2D rigidbody = target.GetComponent<Rigidbody2D>();
+                if (rigidbody != null)
+                    return tpos + rigidbody.velocity * targetPredictionTime;
+            }
+            return tpos;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/Follower.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: d3b58755f80ab91479f1f9314acdea17
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/FootStepSounds.cs

@@ -0,0 +1,90 @@
+using System;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Play foot steps depending on the NavTags at the agents current position.
+    /// </summary>
+    public class FootStepSounds : MonoBehaviour
+    {
+        public AudioClip[] FootStepSoundClips
+        {
+            get => footstepSounds;
+            set
+            {
+                if (value == null)
+                    throw new ArgumentNullException();
+                if (value.Length != PathBerserker2dSettings.NavTags.Length)
+                    throw new ArgumentException($"FootStepSoundClips needs to be an array of length equal to the amount of NavTags ({PathBerserker2dSettings.NavTags.Length}).");
+
+                footstepSounds = value;
+            }
+        }
+
+        [SerializeField]
+        public AudioSource audioSource = null;
+
+        [SerializeField]
+        public NavAgent agent = null;
+
+        /// <summary>
+        /// Delay between playing of footstep sounds.
+        /// </summary>
+        [SerializeField]
+        public float footStepDelay = 1f;
+
+        /// <summary>
+        /// Used when no NavTag specific footStep was found, or if the current segment has no NavTag.
+        /// </summary>
+        [SerializeField]
+        public AudioClip defaultFootstep = null;
+
+        /// <summary>
+        /// Footsteps to use for each NavTag.
+        /// </summary>
+        [SerializeField]
+        AudioClip[] footstepSounds = null;
+
+        private float lastFootStepTime;
+
+        void Update()
+        {
+            // time to play next step? Is agent moving on segment?
+            if (Time.time - lastFootStepTime >= footStepDelay && agent.IsMovingOnSegment)
+            {
+                lastFootStepTime = Time.time;
+                int navTagV = agent.CurrentNavTagVector;
+
+                AudioClip chosenClip = defaultFootstep;
+                // chose the first step sound with matching NavTag
+                for (int i = 0; i < footstepSounds.Length; i++)
+                {
+                    if ((navTagV & (1 << i)) != 0)
+                    {
+                        chosenClip = footstepSounds[i];
+                        break;
+                    }
+                }
+                audioSource.PlayOneShot(chosenClip);
+            }
+        }
+
+        private void OnValidate()
+        {
+            if (footstepSounds == null)
+            {
+                footstepSounds = new AudioClip[PathBerserker2dSettings.NavTags.Length];
+            }
+            if (footstepSounds.Length != PathBerserker2dSettings.NavTags.Length)
+            {
+                System.Array.Resize(ref footstepSounds, PathBerserker2dSettings.NavTags.Length);
+            }
+        }
+
+        private void Reset()
+        {
+            agent = GetComponent<NavAgent>();
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/FootStepSounds.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: c5f023eaead09b849a79459b344a457f
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/KeepGrounded.cs

@@ -0,0 +1,35 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Keeps the agent on moving platforms, by parenting the agent to them.
+    /// </summary>
+    public class KeepGrounded : MonoBehaviour
+    {
+        [SerializeField]
+        public LayerMask movingPlatformLayermask = 0;
+
+        Transform originalParent;
+
+        private void Awake()
+        {
+            originalParent = transform.parent;
+        }
+
+        void FixedUpdate()
+        {
+            var hit = Physics2D.Raycast(transform.position + transform.up * 0.1f, -transform.up, 0.4f, movingPlatformLayermask);
+            if (hit.collider != null)
+            {
+                // we hit a moving platform -> parent
+                transform.SetParent(hit.collider.transform, true);
+            }
+            else
+            {
+                // we didn't hit a moving platform -> unparent
+                transform.SetParent(originalParent, true);
+            }
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/KeepGrounded.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: db3357009171fec4db2b712ce84cd1b5
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/MouseWalker.cs

@@ -0,0 +1,45 @@
+using UnityEngine;
+#if ENABLE_INPUT_SYSTEM && !ENABLE_LEGACY_INPUT_MANAGER
+using UnityEngine.InputSystem;
+#endif
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Let the NavAgent walk to a mouse click.
+    /// </summary>
+    class MouseWalker : MonoBehaviour
+    {
+        [SerializeField]
+        public NavAgent navAgent;
+
+        void Update()
+        {
+            // mouse click occurred?
+#if ENABLE_INPUT_SYSTEM && !ENABLE_LEGACY_INPUT_MANAGER
+            if (Mouse.current.leftButton.wasPressedThisFrame)
+#else
+            if (Input.GetMouseButtonDown(0))
+#endif
+            {
+#if ENABLE_INPUT_SYSTEM && !ENABLE_LEGACY_INPUT_MANAGER
+                Vector2 pos = Camera.main.ScreenToWorldPoint(Mouse.current.position.ReadValue());
+#else
+                Vector2 pos = Camera.main.ScreenToWorldPoint(Input.mousePosition);
+#endif
+                if (!navAgent.PathTo(pos))
+                {
+                    if (navAgent.HasValidPosition)
+                        Debug.Log($"{name}: Pathfinding failed.");
+                    else
+                        Debug.Log($"{name}: Agent is not on a NavSurface.");
+                }
+            }
+        }
+
+        private void Reset()
+        {
+            navAgent = GetComponent<NavAgent>();
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/MouseWalker.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 2a20f61a67733b34ba53481d70603e95
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/MultiGoalWalker.cs

@@ -0,0 +1,52 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Let the agent walk to the closest of the given goals.
+    /// </summary>
+    public class MultiGoalWalker : MonoBehaviour
+    {
+        [SerializeField]
+        public NavAgent navAgent;
+        [SerializeField]
+        Transform[] goals = null;
+        [SerializeField]
+        public bool activateOnStart = true;
+
+        void Start()
+        {
+            if (activateOnStart)
+            {
+                MoveToClosestGoal();
+            }
+        }
+
+        private void Reset()
+        {
+            navAgent = GetComponent<NavAgent>();
+        }
+
+        /// <summary>
+        /// Starts moving to closest of this.goals.
+        /// </summary>
+        public void MoveToClosestGoal()
+        {
+            Vector2[] vs = new Vector2[goals.Length];
+            for (int i = 0; i < goals.Length; i++)
+                vs[i] = goals[i].position;
+            navAgent.PathTo(vs);
+        }
+
+        /// <summary>
+        /// Starts moving to closest of supplied goals.
+        /// </summary>
+        public void MoveToClosestGoal(Transform[] goals)
+        {
+            Vector2[] vs = new Vector2[goals.Length];
+            for (int i = 0; i < goals.Length; i++)
+                vs[i] = goals[i].position;
+            navAgent.PathTo(vs);
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/MultiGoalWalker.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 65ba0666f6ead714a9a712c134268969
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/PatrolWalker.cs

@@ -0,0 +1,79 @@
+using System;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Let the NavAgent walk to a series of goals in a loop.
+    /// </summary>
+    public class PatrolWalker : MonoBehaviour
+    {
+        /// <summary>
+        /// Radius at which the agent starts calculating path to next goal on patrol route. Must be >= 0.
+        /// </summary>
+        public float CalcNextPathRad
+        {
+            get => calcNextPathRad;
+            set
+            {
+                if (calcNextPathRad < 0)
+                    throw new ArgumentException("CalcNextPathRad must be greater or equal to 0");
+                calcNextPathRad = value;
+            }
+        }
+
+        public Transform[] PatrolRoute
+        {
+            get => goals;
+            set
+            {
+                this.goals = value;
+                this.currentGoal = 0;
+            }
+        }
+
+        [SerializeField]
+        public NavAgent navAgent;
+        [SerializeField]
+        Transform[] goals = null;
+        [SerializeField]
+        float calcNextPathRad = 0.2f;
+
+        private Transform goal => goals[currentGoal];
+
+        int currentGoal = 0;
+
+        private void Start()
+        {
+            navAgent.PathTo(goal.position);
+        }
+
+        void Update()
+        {
+            if (goals == null)
+                return;
+
+            // close enough move to next
+            float dist = Vector2.Distance(navAgent.Position, goal.position);
+            if (dist < calcNextPathRad)
+            {
+                currentGoal++;
+                if (currentGoal >= goals.Length)
+                {
+                    currentGoal = 0;
+                }
+                navAgent.UpdatePath(goal.position);
+            }
+        }
+
+        private void OnValidate()
+        {
+            calcNextPathRad = Mathf.Max(calcNextPathRad, 0.1f);
+        }
+
+        private void Reset()
+        {
+            navAgent = GetComponent<NavAgent>();
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/PatrolWalker.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: e2c32017511f38b48a9b544492b5d0f6
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/RandomWalker.cs

@@ -0,0 +1,53 @@
+using System.Collections;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Lets the NavAgent walk to a random point
+    /// </summary>
+    public class RandomWalker : MonoBehaviour
+    {
+        [SerializeField]
+        public NavAgent navAgent;
+
+        /// <summary>
+        /// The random destination my not always be reachable. RetryCount determines the maximum amount of rolls for a random reachable position.
+        /// </summary>
+        [SerializeField, Tooltip("The random destination may not always be reachable. RetryCount determines the maximum amount of rolls each update for a random reachable position.")]
+        public int retryCount = 10;
+
+        /// <summary>
+        /// Will make the agent pick a new random position to walk to, after reaching the previous one. Makes the agent walk between random points, until its set to false.
+        /// </summary>
+        [SerializeField]
+        public bool keepWalkingRandomly = true;
+
+        void Update()
+        {
+            if (keepWalkingRandomly && navAgent.IsIdle)
+            {
+                StartRandomWalk();
+            }
+        }
+
+        private void Reset()
+        {
+            navAgent = GetComponent<NavAgent>();
+        }
+
+        /// <summary>
+        /// Picks a random position and makes the NavAgent walk to it.
+        /// </summary>
+        /// <returns>True, if a random reachable position was found within a maximum of retryCount tries.</returns>
+        public bool StartRandomWalk()
+        {
+            for (int i = 0; i < retryCount && !navAgent.SetRandomDestination(); i++)
+            {
+                
+            }
+
+            return !navAgent.IsIdle;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/NavAgentUsers/RandomWalker.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 9fc605c0d97145a45be2755ef52445e0
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/TransformBasedMovement.cs

@@ -0,0 +1,369 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Moves a NavAgent by manipulating its transform.
+    /// </summary>
+    public class TransformBasedMovement : MonoBehaviour
+    {
+        [System.Flags]
+        public enum FeatureFlags
+        {
+            SegmentMovement = 1,
+            JumpLinks = 2,
+            CornerLinks = 4,
+            FallLinks = 8,
+            TeleportLinks = 16,
+            ClimbLinks = 32,
+            ElevatorLinks = 64,
+            OtherLinks = 128,
+        }
+
+        [Tooltip("Speed on segments in unit/s.")]
+        [SerializeField]
+        public float movementSpeed = 5;
+
+        [Tooltip("Speed on corner links in degrees/s.")]
+        [SerializeField]
+        public float cornerSpeed = 100;
+
+        [Tooltip("Speed on jump links in unit/s.")]
+        [SerializeField]
+        public float jumpSpeed = 5;
+
+        [Tooltip("Speed on fall links in unit/s.")]
+        [SerializeField]
+        public float fallSpeed = 5;
+
+        [Tooltip("Speed on climb links in unit/s.")]
+        [SerializeField]
+        public float climbSpeed = 5;
+
+        /// <summary>
+        /// If false, agent will not be rotated.
+        /// </summary>
+        [Tooltip("Controls whether the default movement handler is allowed to rotate the agent.")]
+        [SerializeField]
+        public bool enableAgentRotation = true;
+
+        /// <summary>
+        /// Sets which links and segments this component will handle. Useful to override an Agents default behavior for a certain link type or segment.
+        /// </summary>
+        [Tooltip("Enable features by setting the flag.")]
+        [SerializeField]
+        public FeatureFlags enabledFeatures = (FeatureFlags)int.MaxValue;
+
+        private float timeOnLink;
+        private float timeToCompleteLink;
+        private Vector2 direction;
+        private int state = 0;
+        private Transform elevatorTrans;
+        private float deltaDistance;
+        private bool handleLinkMovement;
+        private int minNumberOfLinkExecutions;
+        private Vector2 storedLinkStart;
+
+        private void OnEnable()
+        {
+            var agent = GetComponent<NavAgent>();
+            agent.OnStartLinkTraversal += Agent_StartLinkTraversalEvent;
+            agent.OnStartSegmentTraversal += Agent_OnStartSegmentTraversal;
+            agent.OnSegmentTraversal += Agent_OnSegmentTraversal;
+            agent.OnLinkTraversal += Agent_OnLinkTraversal;
+        }
+
+        private void OnDisable()
+        {
+            var agent = GetComponent<NavAgent>();
+            agent.OnStartLinkTraversal -= Agent_StartLinkTraversalEvent;
+            agent.OnStartSegmentTraversal -= Agent_OnStartSegmentTraversal;
+            agent.OnSegmentTraversal -= Agent_OnSegmentTraversal;
+            agent.OnLinkTraversal -= Agent_OnLinkTraversal;
+        }
+
+        private void OnValidate()
+        {
+            if (jumpSpeed <= 0)
+                jumpSpeed = 0.01f;
+            if (fallSpeed <= 0)
+                fallSpeed = 0.01f;
+            if (climbSpeed <= 0)
+                climbSpeed = 0.01f;
+        }
+
+        private void Agent_OnStartSegmentTraversal(NavAgent agent)
+        {
+
+        }
+
+        private void Agent_OnSegmentTraversal(NavAgent agent)
+        {
+            if (!enabledFeatures.HasFlag(FeatureFlags.SegmentMovement))
+                return;
+
+            Vector2 newPos;
+            bool reachedGoal = MoveAlongSegment(agent.Position, agent.PathSubGoal, agent.CurrentPathSegment.Point, agent.CurrentPathSegment.Tangent, Time.deltaTime * movementSpeed, out newPos);
+            agent.Position = newPos;
+            
+            if (reachedGoal)
+            {
+                agent.CompleteSegmentTraversal();
+            }
+        }
+
+        private void Agent_StartLinkTraversalEvent(NavAgent agent)
+        {
+            string linkType = agent.CurrentPathSegment.link.LinkTypeName;
+
+            bool unknownLinkType = linkType != "corner" && linkType != "fall" && linkType != "jump" && linkType != "elevator" && linkType != "teleport" && linkType != "climb";
+
+            handleLinkMovement =
+                (unknownLinkType && enabledFeatures.HasFlag(FeatureFlags.OtherLinks)) || 
+                (linkType == "corner" && enabledFeatures.HasFlag(FeatureFlags.CornerLinks)) ||
+                (linkType == "fall" && enabledFeatures.HasFlag(FeatureFlags.FallLinks)) ||
+                (linkType == "jump" && enabledFeatures.HasFlag(FeatureFlags.JumpLinks)) ||
+                (linkType == "elevator" && enabledFeatures.HasFlag(FeatureFlags.ElevatorLinks)) ||
+                (linkType == "teleport" && enabledFeatures.HasFlag(FeatureFlags.TeleportLinks)) ||
+                (linkType == "climb" && enabledFeatures.HasFlag(FeatureFlags.ClimbLinks));
+
+            if (!handleLinkMovement)
+                return;
+
+            timeOnLink = 0;
+            Vector2 delta = agent.PathSubGoal - agent.CurrentPathSegment.LinkStart;
+            deltaDistance = delta.magnitude;
+            direction = delta / deltaDistance;
+            minNumberOfLinkExecutions = 1;
+            storedLinkStart = agent.CurrentPathSegment.LinkStart;
+
+            float speed = 1;
+            switch (agent.CurrentPathSegment.link.LinkTypeName)
+            {
+                case "corner":
+                    if (!enableAgentRotation)
+                    {
+                        agent.CompleteLinkTraversal();
+                        break;
+                    }
+                    speed = cornerSpeed;
+                    deltaDistance = agent.CurrentPathSegment.link.TravelCosts(Vector2.zero, Vector2.zero);
+                    break;
+                case "fall":
+                    speed = fallSpeed;
+                    break;
+                case "climb":
+                    speed = climbSpeed;
+
+                    Vector2 pos = agent.CurrentPathSegment.link.GameObject.transform.position;
+                    Vector2 dir = agent.CurrentPathSegment.link.GameObject.transform.up;
+
+                    Vector2 start = Geometry.ProjectPointOnLine(agent.CurrentPathSegment.LinkStart, pos, dir);
+                    Vector2 end = Geometry.ProjectPointOnLine(agent.PathSubGoal, pos, dir);
+                    deltaDistance = Vector2.Distance(start, pos) + Vector2.Distance(start, end) + Vector2.Distance(end, agent.PathSubGoal);
+
+                    state = 0;
+                    minNumberOfLinkExecutions = 3;
+                    break;
+                case "jump":
+                    speed = jumpSpeed;
+                    break;
+                case "elevator":
+                    speed = movementSpeed;
+                    state = 0;
+                    minNumberOfLinkExecutions = 4;
+                    elevatorTrans = agent.CurrentPathSegment.link.GameObject.transform;
+                    var childTrans = agent.CurrentPathSegment.link.GameObject.GetComponentsInChildren<Transform>();
+                    foreach (var t in childTrans)
+                    {
+                        if (t.gameObject.layer == 8)
+                        {
+                            elevatorTrans = t;
+                            break;
+                        }
+                    }
+                    break;
+            }
+
+            if (agent.CurrentPathSegment.link.LinkTypeName == "elevator")
+                timeToCompleteLink = float.PositiveInfinity;
+            else
+                timeToCompleteLink = (deltaDistance / speed);
+        }
+
+        private void Agent_OnLinkTraversal(NavAgent agent)
+        {
+            if (!handleLinkMovement)
+                return;
+
+            timeOnLink += Time.deltaTime;
+            timeOnLink = Mathf.Min(timeToCompleteLink, timeOnLink);
+
+            switch (agent.CurrentPathSegment.link.LinkTypeName)
+            {
+                case "corner":
+                    Corner(agent);
+                    break;
+                case "jump":
+                    Jump(agent);
+                    break;
+                case "fall":
+                    Fall(agent);
+                    break;
+                case "teleport":
+                    Teleport(agent);
+                    timeOnLink = timeToCompleteLink + 1;
+                    break;
+                case "climb":
+                    Climb(agent);
+                    break;
+                case "elevator":
+                    Elevator(agent);
+                    break;
+                default:
+                    Jump(agent);
+                    break;
+            }
+
+            minNumberOfLinkExecutions--;
+            if (timeOnLink >= timeToCompleteLink && minNumberOfLinkExecutions <= 0)
+            {
+                agent.CompleteLinkTraversal();
+                return;
+            }
+        }
+
+        private void Corner(NavAgent agent)
+        {
+            var from = Quaternion.LookRotation(Vector3.forward, agent.CurrentPathSegment.Normal);
+            var to = Quaternion.LookRotation(Vector3.forward, agent.CurrentPathSegment.Next.Normal);
+
+
+            agent.transform.rotation = Quaternion.Slerp(
+                from,
+                to,
+                agent.TimeOnLink / (deltaDistance / cornerSpeed));
+        }
+
+        private void Jump(NavAgent agent)
+        {
+            Vector2 newPos = storedLinkStart + direction * timeOnLink * jumpSpeed;
+            newPos.y += deltaDistance * 0.3f * Mathf.Sin(Mathf.PI * timeOnLink / timeToCompleteLink);
+            agent.Position = newPos;
+        }
+
+        private void Fall(NavAgent agent)
+        {
+            Vector2 newPos = storedLinkStart + direction * timeOnLink * fallSpeed;
+            agent.Position = newPos;
+        }
+
+        private void Climb(NavAgent agent)
+        {
+            Vector2 linkPos = agent.CurrentPathSegment.link.GameObject.transform.position;
+            Vector2 linkDir = agent.CurrentPathSegment.link.GameObject.transform.up;
+
+            Vector2 newPos = Vector2.zero;
+            switch (state)
+            {
+                case 0:
+                    Vector2 start = Geometry.ProjectPointOnLine(agent.CurrentPathSegment.LinkStart, linkPos, linkDir);
+                    if (MoveTo(agent.Position, start, climbSpeed * Time.deltaTime, out newPos))
+                    {
+                        state = 1;
+                    }
+                    break;
+                case 1:
+                    Vector2 end = Geometry.ProjectPointOnLine(agent.PathSubGoal, linkPos, linkDir);
+                    if (MoveTo(agent.Position, end, climbSpeed * Time.deltaTime, out newPos))
+                    {
+                        state = 2;
+                    }
+                    break;
+                case 2:
+                    if (MoveTo(agent.Position, agent.PathSubGoal, climbSpeed * Time.deltaTime, out newPos))
+                    {
+                        // force early exit
+                        timeToCompleteLink = 0;
+                    }
+                    break;
+            }
+            agent.Position = newPos;
+        }
+
+        private void Elevator(NavAgent agent)
+        {
+            // 3 phase
+            // 1. move on elevator
+            // 2. wait to reach destination
+            // 3. leave
+
+            Vector2 newPos = agent.Position;
+            switch (state)
+            {
+                case 0:
+                    Vector2 target = elevatorTrans.position;
+                    if (agent.CurrentPathSegment.link.IsTraversable && Mathf.Abs(newPos.y - target.y) < 0.1f)
+                    {
+                        state = 1;
+                        newPos.y = target.y;
+                        direction = Vector2.right * Mathf.Sign(target.x - storedLinkStart.x);
+                    }
+                    break;
+                case 1:
+                    newPos += movementSpeed * direction * Time.deltaTime;
+
+                    float targetX = agent.CurrentPathSegment.link.GameObject.transform.position.x;
+                    if ((newPos.x - targetX) * direction.x >= 0)
+                    {
+                        state = 2;
+                        newPos.x = targetX;
+                    }
+                    break;
+                case 2:
+                    // wait till y matches elevation
+                    // cast ray downwards to move with platform
+                    float targetY = agent.PathSubGoal.y;
+                    if (agent.CurrentPathSegment.link.IsTraversable && Mathf.Abs(newPos.y - targetY) < 0.1f)
+                    {
+                        state = 3;
+                        newPos.y = targetY;
+                        direction = Vector2.right * Mathf.Sign(agent.PathSubGoal.x - newPos.x);
+                        timeOnLink = 0;
+                        timeToCompleteLink = Mathf.Abs(agent.PathSubGoal.x - newPos.x) / movementSpeed;
+                    }
+                    break;
+                case 3:
+                    newPos += movementSpeed * direction * Time.deltaTime;
+                    break;
+            }
+            agent.Position = newPos;
+        }
+
+        private void Teleport(NavAgent agent)
+        {
+            agent.Position = agent.PathSubGoal;
+        }
+
+        private static bool MoveAlongSegment(Vector2 pos, Vector2 goal, Vector2 segPoint, Vector2 segTangent, float amount, out Vector2 newPos)
+        {
+            pos = Geometry.ProjectPointOnLine(pos, segPoint, segTangent);
+            goal = Geometry.ProjectPointOnLine(goal, segPoint, segTangent);
+            return MoveTo(pos, goal, amount, out newPos);
+        }
+
+        private static bool MoveTo(Vector2 pos, Vector2 goal, float amount, out Vector2 newPos)
+        {
+            Vector2 dir = goal - pos;
+            float distance = dir.magnitude;
+            if (distance <= amount)
+            {
+                newPos = goal;
+                return true;
+            }
+
+            newPos = pos + dir * amount / distance;
+            return false;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavAgent/TransformBasedMovement.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 77030faff3812a7429edeaca91e9c873
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavGraph.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: cd0d5b7789d46e742afeb4aa400c1822
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavGraph/NavGraph.cs

@@ -0,0 +1,831 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Threading;
+using UnityEngine;
+using UnityEngine.XR.WSA;
+
+namespace PathBerserker2d
+{
+    public enum NavGraphChange
+    {
+        NavSurfaceAdded,
+        NavSurfaceRemoved,
+        NavLinkAdded,
+        NavLinkRemoved,
+        SegmentModifierAdded,
+        SegmentModifierRemoved,
+        NavLinkMoved
+    }
+
+    public interface INavGraphChangeSource
+    {
+        /// <summary>
+        /// Called after a group of changes has been applied to the NavGraph. When this is called, the changes are already applied and ready for use. For example NavSurfaceAdded means that that NavSurface is ready to be pathfinded on from now on, link mapping will work etc.
+        /// This may be called multiple times for the segment modifier related events as one SegmentModifier GameObject can produce multiple SegmentModifier changes.
+        /// Parameters:
+        /// - Change type
+        /// - the PBComponentId of the component (NavSurface, Link, SegmentModifier, ...) had. PBComponentId is a custom id that PathBerserker gives each component. Each component (NavSurface, Link, SegmentModifier, ...) has a PBComponentId property you can query to get its id.
+        /// </summary>
+        event Action<NavGraphChange, int> OnGraphChange;
+    }
+
+    internal class NavGraph : INavGraphChangeSource
+    {
+        public ReaderWriterLock graphLock = new ReaderWriterLock();
+        public event Action<NavGraphChange, int> OnGraphChange;
+
+        internal readonly Dictionary<NavSurface, NavSurfaceRecord> segmentTrees = new Dictionary<NavSurface, NavSurfaceRecord>();
+
+        List<IGraphChange> changes = new List<IGraphChange>(20);
+        List<AddNavSurfaceChange> stagedNewSurfaces = new List<AddNavSurfaceChange>(4);
+
+        private int pathfinderThreadCount;
+
+        public NavGraph(int pathfinderThreadCount)
+        {
+            this.pathfinderThreadCount = pathfinderThreadCount;
+        }
+
+        public void AddNavSurface(NavSurface surface)
+        {
+            var addSurface = new AddNavSurfaceChange(surface, pathfinderThreadCount);
+            if (!stagedNewSurfaces.Any(item => item.surface == surface))
+                stagedNewSurfaces.Add(addSurface);
+            changes.Add(addSurface);
+        }
+
+        public void RemoveNavSurface(NavSurface surface)
+        {
+            changes.Add(new RemoveNavSurfaceChange(surface));
+        }
+
+        public void AddNavLink(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal)
+        {
+            changes.Add(new AddNavLinkChange(link, start, goal));
+        }
+
+        public void RemoveNavLink(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal)
+        {
+            changes.Add(new RemoveNavLinkChange(link, start, goal));
+        }
+
+        public void MoveNavLinkStart(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal, NavSegmentPositionPointer oldStart)
+        {
+            changes.Add(new MoveNavLinkStartChange(link, start, goal, oldStart));
+        }
+
+        public void MoveNavLinkGoal(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal, NavSegmentPositionPointer oldGoal)
+        {
+            changes.Add(new MoveNavLinkGoalChange(link, start, goal, oldGoal));
+        }
+
+        public void AddSegmentModifier(NavAreaMarkerInstance modifier)
+        {
+            changes.Add(new AddSegmentModifierChange(modifier));
+        }
+
+        public void RemoveSegmentModifier(NavAreaMarkerInstance modifier)
+        {
+            changes.Add(new RemoveSegmentModifierChange(modifier));
+        }
+
+        public void Update()
+        {
+            // update navsurface position matricies
+            foreach (var entry in segmentTrees)
+            {
+                if (entry.Key != null)
+                    entry.Value.LocalToWorld = entry.Key.LocalToWorldMatrix;
+            }
+
+            if (changes.Count + stagedNewSurfaces.Count == 0)
+                return;
+
+            graphLock.AcquireWriterLock(-1);
+            try
+            {
+                for (int i = 0; i < changes.Count; i++)
+                    changes[i].Apply(this);
+            }
+            finally
+            {
+                graphLock.ReleaseWriterLock();
+            }
+
+            // fire events
+            if (OnGraphChange != null)
+            {
+                for (int i = 0; i < changes.Count; i++)
+                {
+                    try
+                    {
+                        OnGraphChange(changes[i].ChangeType, changes[i].ChangeSource);
+                    }
+                    catch (Exception e)
+                    {
+                        // we don't want listener exceptions to take down the pathfinding system with it
+                        Debug.LogError(e);
+                    }
+                }
+            }
+
+            stagedNewSurfaces.Clear();
+            changes.Clear();
+        }
+
+        public void ForceApplyChanges()
+        {
+            for (int i = 0; i < changes.Count; i++)
+                changes[i].Apply(this);
+
+            stagedNewSurfaces.Clear();
+            changes.Clear();
+        }
+
+        public bool TryMapAgent(Vector2 position, NavSegmentPositionPointer pointer, NavAgent agent, out NavSegmentPositionPointer result)
+        {
+            if (!pointer.IsInvalid() && agent.CouldBeLocatedAt(pointer))
+            {
+                NavGraphNodeCluster cluster;
+                if (TryGetClusterAt(pointer, out cluster))
+                {
+                    Vector2 localPos = pointer.surface.WorldToLocal(position);
+                    float t = cluster.DistanceOfPointAlongSegment(localPos);
+                    Vector2 proj = cluster.GetPositionAlongSegment(t);
+                    float dist = (proj - localPos).sqrMagnitude;
+
+                    if (dist < 0.001f)
+                    {
+                        result = new NavSegmentPositionPointer(pointer.surface, pointer.cluster, cluster.DistanceOfPointAlongSegment(localPos));
+                        return true;
+                    }
+                }
+            }
+
+            return TryMapPoint(position, agent.CouldBeLocatedAt, out result);
+        }
+
+        public bool TryMapPoint(Vector2 position, out NavSegmentPositionPointer pointer)
+        {
+            return TryMapPoint(position, (p) => true, out pointer);
+        }
+
+        public bool TryMapPoint(Vector2 position, Func<NavSegmentPositionPointer, bool> pointFilter, out NavSegmentPositionPointer pointer)
+        {
+            return TryMapPoint(position, pointFilter, PathBerserker2dSettings.PointMappingDistance, out pointer);
+        }
+
+        public bool TryMapPoint(Vector2 position, Func<NavSegmentPositionPointer, bool> pointFilter, float pointMapDistance, out NavSegmentPositionPointer result)
+        {
+            float halfPointMapDistance = pointMapDistance / 2f;
+            Rect queryAABB = new Rect(position - new Vector2(halfPointMapDistance, halfPointMapDistance), new Vector2(pointMapDistance, pointMapDistance));
+            float minDistance = pointMapDistance;
+            result = NavSegmentPositionPointer.Invalid;
+            foreach (var pair in segmentTrees)
+            {
+                TryMapPointSurface(pair.Key, pair.Value.Clusters, queryAABB, position, pointFilter, ref minDistance, ref result);
+            }
+
+            return result.IsValid();
+        }
+
+        public bool TryMapPointWithStaged(Vector2 position, out NavSegmentPositionPointer result)
+        {
+            if (TryMapPoint(position, out result))
+                return true;
+
+            float pointMapDistance = PathBerserker2dSettings.PointMappingDistance;
+            float halfPointMapDistance = pointMapDistance / 2f;
+            Rect queryAABB = new Rect(position - new Vector2(halfPointMapDistance, halfPointMapDistance), new Vector2(pointMapDistance, pointMapDistance));
+            float minDistance = pointMapDistance;
+            result = NavSegmentPositionPointer.Invalid;
+            foreach (var change in stagedNewSurfaces)
+            {
+                TryMapPointSurface(change.surface, change.record.Clusters, queryAABB, position, (p) => true, ref minDistance, ref result);
+            }
+
+            return result.IsValid();
+        }
+
+        public List<NavSubsegmentPointer> BoxCast(Rect rect, float rotation, float filterAngleFrom, float filterAngleTo)
+        {
+            List<NavSubsegmentPointer> results = new List<NavSubsegmentPointer>();
+
+            // find bb of rect
+            Vector2[] rotCorn = ExtendedGeometry.RotateRectangle(rect, rotation);
+
+            Vector2 min = Vector2.Min(Vector2.Min(Vector2.Min(rotCorn[0], rotCorn[1]), rotCorn[2]), rotCorn[3]);
+            Vector2 max = Vector2.Max(Vector2.Max(Vector2.Max(rotCorn[0], rotCorn[1]), rotCorn[2]), rotCorn[3]);
+            Rect boundingRect = new Rect(min, max - min);
+
+            Matrix4x4 rotationMatrix = Matrix4x4.Rotate(Quaternion.Euler(0, 0, rotation));
+
+            foreach (var pair in segmentTrees)
+            {
+                BoxCastSurface(pair.Key, pair.Value.Clusters, rect, boundingRect, rotationMatrix, filterAngleFrom, filterAngleTo, ref results);
+            }
+            return results;
+        }
+
+        public List<NavSubsegmentPointer> BoxCastWithStaged(Rect rect, float rotation, float filterAngleFrom, float filterAngleTo)
+        {
+            List<NavSubsegmentPointer> results = BoxCast(rect, rotation, filterAngleFrom, filterAngleTo);
+
+            // find bb of rect
+            Vector2[] rotCorn = ExtendedGeometry.RotateRectangle(rect, rotation);
+
+            Vector2 min = Vector2.Min(Vector2.Min(Vector2.Min(rotCorn[0], rotCorn[1]), rotCorn[2]), rotCorn[3]);
+            Vector2 max = Vector2.Max(Vector2.Max(Vector2.Max(rotCorn[0], rotCorn[1]), rotCorn[2]), rotCorn[3]);
+            Rect boundingRect = new Rect(min, max - min);
+
+            Matrix4x4 rotationMatrix = Matrix4x4.Rotate(Quaternion.Euler(0, 0, rotation));
+
+            foreach (var change in stagedNewSurfaces)
+            {
+                BoxCastSurface(change.surface, change.record.Clusters, rect, boundingRect, rotationMatrix, filterAngleFrom, filterAngleTo, ref results);
+            }
+            return results;
+        }
+
+        [Obsolete("Use TryMapPoint instead")]
+        public bool TryFindClosestPointTo(Vector2 position, float maxMappingDistance, out NavSegmentPositionPointer pointer)
+        {
+            return TryMapPoint(position, (p) => true, maxMappingDistance, out pointer);
+        }
+
+        public Vector2 GetRandomPointOnGraph()
+        {
+            if (segmentTrees.Count == 0)
+                return Vector2.zero;
+
+            var surf = Utility.WeightedRandomChoice(segmentTrees.Keys,
+                segmentTrees.Keys.Select(s => s.TotalLineLength));
+
+            var seg = Utility.WeightedRandomChoice(surf.NavSegments,
+                surf.NavSegments.Select(s => s.Length), surf.TotalLineLength);
+
+            var t = UnityEngine.Random.Range(0, seg.Length);
+
+            return surf.LocalToWorld(seg.GetPositionAlongSegment(t));
+        }
+
+        private void TryMapPointSurface(NavSurface surface, B2DynamicTree<NavGraphNodeCluster> tree, Rect queryAABB, Vector2 position, Func<NavSegmentPositionPointer, bool> filter, ref float minDistance, ref NavSegmentPositionPointer result)
+        {
+            if (!surface.WorldBounds.Overlaps(queryAABB))
+            {
+                return;
+            }
+
+            Vector2 localPoint = surface.WorldToLocal(position);
+            Rect localQuery = new Rect(surface.WorldToLocal(queryAABB.position), queryAABB.size);
+
+            var iterator = tree.Query(localQuery);
+            while (iterator.MoveNext())
+            {
+                var clusterCandidate = tree.GetUserData(iterator.Current);
+                float t = clusterCandidate.DistanceOfPointAlongSegment(localPoint);
+                Vector2 proj = clusterCandidate.GetPositionAlongSegment(t);
+                float dist = (proj - localPoint).magnitude;
+
+                if (dist >= minDistance)
+                    // closest point is already to far away
+                    continue;
+
+                if (filter(new NavSegmentPositionPointer(surface, clusterCandidate, t)))
+                {
+                    minDistance = dist;
+                    result = new NavSegmentPositionPointer(surface, clusterCandidate, t);
+                }
+                else
+                {
+                    // sub sample segment
+                    for (t = 0; t < clusterCandidate.Length; t += 0.5f)
+                    {
+                        if (filter(new NavSegmentPositionPointer(surface, clusterCandidate, t)))
+                        {
+                            proj = clusterCandidate.GetPositionAlongSegment(t);
+                            dist = (proj - localPoint).magnitude;
+
+                            if (dist >= minDistance)
+                                continue;
+
+                            result = new NavSegmentPositionPointer(surface, clusterCandidate, t);
+                            minDistance = dist;
+                            break;
+                        }
+                    }
+                }
+            }
+        }
+
+        private void BoxCastSurface(NavSurface surface, B2DynamicTree<NavGraphNodeCluster> tree, Rect rect, Rect boundingRect, Matrix4x4 rotationMatrix, float filterAngleFrom, float filterAngleTo, ref List<NavSubsegmentPointer> results)
+        {
+            if (!surface.WorldBounds.Overlaps(boundingRect))
+                return;
+
+            Rect localQuery = new Rect(boundingRect);
+            localQuery.center = surface.WorldToLocal(localQuery.center);
+
+            Rect localCast = new Rect(rect);
+            //localCast.center = surface.WorldToLocal(localCast.center);
+
+            float u1, u2;
+            var iterator = tree.Query(localQuery);
+            while (iterator.MoveNext())
+            {
+                var segCandidate = tree.GetUserData(iterator.Current);
+
+                float angle = Vector2.SignedAngle(segCandidate.Tangent, Vector2.up);
+                if (!ExtendedGeometry.IsAngleBetweenAngles(filterAngleFrom, filterAngleTo, angle))
+                    continue;
+
+                // test for rect intersection
+                Vector2 rotatedStart = rotationMatrix * surface.LocalToWorld(segCandidate.Start);
+                Vector2 rotatedEnd = rotationMatrix * surface.LocalToWorld(segCandidate.End);
+                if (ExtendedGeometry.RectLineIntersection(localCast, rotatedStart, rotatedEnd, out u1, out u2))
+                {
+                    u1 = u1 < 0 ? 0 : u1;
+                    u2 = u2 > 1 ? 1 : u2;
+                    u1 *= segCandidate.Length;
+                    u2 *= segCandidate.Length;
+
+                    results.Add(new NavSubsegmentPointer(surface, iterator.Current, u1, u2 - u1));
+                }
+            }
+        }
+
+        private void InternalAddNavSurface(NavSurface surface,
+            NavSurfaceRecord record, int[] proxyIndecies)
+        {
+            if (segmentTrees.ContainsKey(surface))
+            {
+#if PBDEBUG
+                Debug.Log("AddNavSurface called but surface was already added!");
+#endif
+                return;
+            }
+
+            // add corner links for connected segments
+            for (int iSeg = 0; iSeg < surface.NavSegments.Count; iSeg++)
+            {
+                var seg = surface.NavSegments[iSeg];
+                var cluster = record.Clusters.GetUserData(proxyIndecies[iSeg]);
+
+                if (seg.HasNext)
+                {
+                    var goalCluster = record.Clusters.GetUserData(proxyIndecies[seg.NextSegmentIndex]);
+
+                    AddCornerLink(cluster, cluster.Length, surface, goalCluster, 0);
+                }
+                if (seg.HasPrev)
+                {
+                    var goalCluster = record.Clusters.GetUserData(proxyIndecies[seg.PrevSegmentIndex]);
+
+                    AddCornerLink(cluster, 0, surface, goalCluster, goalCluster.Length);
+                }
+            }
+
+            // add corner links for touching segments
+            for (int iSeg = 0; iSeg < surface.NavSegments.Count; iSeg++)
+            {
+                var seg = surface.NavSegments[iSeg];
+                int proxyIndex = proxyIndecies[iSeg];
+                var startCluster = record.Clusters.GetUserData(proxyIndex);
+
+                CreateCornerLinksForClosePoints(seg.Start, startCluster, proxyIndex, record.Clusters, proxyIndecies, surface);
+                CreateCornerLinksForClosePoints(seg.End, startCluster, proxyIndex, record.Clusters, proxyIndecies, surface);
+            }
+
+            segmentTrees.Add(surface, record);
+        }
+
+        private void AddCornerLink(NavGraphNodeCluster startCluster, float startT, NavSurface startSurface, NavGraphNodeCluster goalCluster, float goalT)
+        {
+            float angle = Vector2.Angle(startCluster.Normal, goalCluster.Normal);
+            angle = angle < 0 ? 360 - angle : angle;
+
+            var start = new NavSegmentPositionPointer(startSurface, startCluster, startT);
+            var goal = new NavSegmentPositionPointer(startSurface, goalCluster, goalT);
+
+            startCluster.AddNode(pathfinderThreadCount, start.t, goal.cluster, goal.t, new CornerLink(start, goal, angle));
+        }
+
+        private void CreateCornerLinksForClosePoints(Vector2 point, NavGraphNodeCluster pointOwner, int pointOwnerProxyIndex, B2DynamicTree<NavGraphNodeCluster> clusterTree, int[] proxyIndicies, NavSurface surface)
+        {
+            int prevSegProxyIndex = pointOwner.HasPrev ? proxyIndicies[pointOwner.PrevSegmentIndex] : -1;
+
+            Vector2 queryRectSize = new Vector2(0.01f, 0.01f);
+            Rect query = new Rect(point - queryRectSize, point + queryRectSize);
+            var iterator = clusterTree.Query(query);
+
+            while (iterator.MoveNext())
+            {
+                if (prevSegProxyIndex == iterator.Current || iterator.Current == pointOwnerProxyIndex)
+                    continue;
+
+                var goalCluster = clusterTree.GetUserData(iterator.Current);
+                if (goalCluster.PointDistance(point) < 0.01f)
+                {
+                    float startT = pointOwner.DistanceOfPointAlongSegment(point);
+                    float goalT = goalCluster.DistanceOfPointAlongSegment(point);
+
+                    AddCornerLink(pointOwner, startT, surface, goalCluster, goalT);
+                    AddCornerLink(goalCluster, goalT, surface, pointOwner, startT);
+                }
+            }
+        }
+
+        private bool DoesLinkExist(INavLinkInstance link, NavSegmentPositionPointer start)
+        {
+            NavGraphNodeCluster cluster;
+            if (!TryGetClusterAt(start, out cluster))
+            {
+                return false;
+            }
+            return cluster.DoesNodeExist(link);
+        }
+
+        private void InternalRemoveNavSurface(NavSurface surface)
+        {
+            NavSurfaceRecord record = null;
+            try
+            {
+                record = segmentTrees[surface];
+
+            }
+            catch (KeyNotFoundException)
+            {
+#if PBDEBUG
+                Debug.Log("RemoveNavSurface called but surface doesn't exist!");
+#endif
+                return;
+            }
+            record.Destroy(this);
+            segmentTrees.Remove(surface);
+        }
+
+        private void InternalAddNavLink(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal)
+        {
+            start.cluster.AddNode(
+                 pathfinderThreadCount,
+                 start.t,
+                 goal.cluster,
+                 goal.t,
+                 link);
+
+            CreateSoftRefLink(link, start.surface);
+            if (start.surface != goal.surface)
+            {
+                CreateSoftRefLink(link, goal.surface);
+            }
+        }
+
+        private void CreateSoftRefLink(INavLinkInstance link, NavSurface targetSurface)
+        {
+            try
+            {
+                segmentTrees[targetSurface].AddSoftRefLink(link);
+            }
+            catch (KeyNotFoundException)
+            {
+                // must be still staged
+                for (int i = 0; i < stagedNewSurfaces.Count; i++)
+                {
+                    if (stagedNewSurfaces[i].surface == targetSurface)
+                    {
+                        stagedNewSurfaces[i].record.AddSoftRefLink(link);
+                        return;
+                    }
+                }
+
+                throw new KeyNotFoundException();
+            }
+        }
+
+        private void RemoveSoftRefLink(INavLinkInstance link, NavSurface targetSurface)
+        {
+            if (targetSurface == null)
+                return;
+            try
+            {
+                segmentTrees[targetSurface].RemoveSoftRefLink(link);
+            }
+            catch (KeyNotFoundException)
+            {
+                // must be still staged
+                for (int i = 0; i < stagedNewSurfaces.Count; i++)
+                {
+                    if (stagedNewSurfaces[i].surface == targetSurface)
+                    {
+                        stagedNewSurfaces[i].record.RemoveSoftRefLink(link);
+                        return;
+                    }
+                }
+
+                throw new KeyNotFoundException();
+            }
+        }
+
+        internal void InternalRemoveNavLink(INavLinkInstance link, NavSegmentPositionPointer start,
+        NavSegmentPositionPointer goal)
+        {
+            if (start.surface == null || !DoesLinkExist(link, start))
+                return;
+
+            start.cluster.RemoveNode(link);
+            RemoveSoftRefLink(link, start.surface);
+            if (start.surface != goal.surface && segmentTrees.ContainsKey(goal.surface))
+                RemoveSoftRefLink(link, goal.surface);
+
+            link.OnRemove();
+        }
+
+        private void InternalMoveNavLinkStart(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal, NavSegmentPositionPointer oldStart)
+        {
+            if (oldStart.surface == start.surface && oldStart.cluster == start.cluster)
+            {
+                oldStart.cluster.MoveNode(link, start.t);
+            }
+            else
+            {
+                oldStart.cluster.RemoveNode(link);
+                RemoveSoftRefLink(link, oldStart.surface);
+                if (oldStart.surface != goal.surface)
+                    RemoveSoftRefLink(link, goal.surface);
+
+                InternalAddNavLink(link, start, goal);
+            }
+        }
+
+        private void InternalMoveNavLinkGoal(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal, NavSegmentPositionPointer oldGoal)
+        {
+            var node = start.cluster.GetNode(link);
+            if (oldGoal.surface != goal.surface || oldGoal.cluster != goal.cluster)
+                node.LinkTarget = goal.cluster;
+            node.LinkTargetT = goal.t;
+
+            if (goal.surface != oldGoal.surface)
+            {
+                if (start.surface != oldGoal.surface)
+                    RemoveSoftRefLink(link, oldGoal.surface);
+
+                if (start.surface != goal.surface)
+                    CreateSoftRefLink(link, goal.surface);
+            }
+        }
+
+        private void InternalAddSegmentModifier(NavAreaMarkerInstance mod)
+        {
+            if (TryGetClusterAt(mod.position, out var cluster))
+                cluster.AddNodeClusterModifier(mod);
+        }
+
+        private void InternalRemoveSegmentModifier(NavAreaMarkerInstance mod)
+        {
+            if (TryGetClusterAt(mod.position, out var cluster))
+                cluster.RemoveNodeClusterModifier(mod);
+        }
+
+        public bool TryGetClusterAt(NavSegmentPositionPointer tPoint, out NavGraphNodeCluster cluster)
+        {
+            if (tPoint.IsInvalid() || !segmentTrees.TryGetValue(tPoint.surface, out _))
+            {
+                cluster = null;
+                return false;
+            }
+
+            cluster = tPoint.cluster;
+            return true;
+        }
+
+        public bool TryGetClusterAt(NavSubsegmentPointer tPoint, out NavGraphNodeCluster cluster)
+        {
+            NavSurfaceRecord navRec;
+            if (tPoint.IsInvalid() || !segmentTrees.TryGetValue(tPoint.surface, out navRec))
+            {
+                cluster = null;
+                return false;
+            }
+
+            cluster = navRec.Clusters.GetUserData(tPoint.proxyDataIndex);
+            return true;
+        }
+
+        interface IGraphChange
+        {
+            NavGraphChange ChangeType { get; }
+            int ChangeSource { get; }
+            void Apply(NavGraph graph);
+        }
+
+        class AddNavSurfaceChange : IGraphChange
+        {
+            public NavGraphChange ChangeType => NavGraphChange.NavSurfaceAdded;
+            public int ChangeSource { get; }
+
+            public readonly NavSurfaceRecord record;
+            public readonly NavSurface surface;
+            int[] proxyIndecies;
+
+            public AddNavSurfaceChange(NavSurface surface, int threadCount)
+            {
+                this.surface = surface;
+                this.ChangeSource = surface.PBComponentId;
+
+                var tree = new B2DynamicTree<NavGraphNodeCluster>(surface.NavSegments.Count + 10);
+
+                proxyIndecies = new int[surface.NavSegments.Count];
+                record = new NavSurfaceRecord(tree, surface.LocalToWorldMatrix, surface);
+                int fill = 0;
+                foreach (var seg in surface.NavSegments)
+                {
+                    proxyIndecies[fill++] = tree.CreateProxy(seg.AABB, new NavGraphNodeCluster(seg, threadCount, record));
+                }
+            }
+
+            public void Apply(NavGraph graph)
+            {
+#if PBDEBUG
+                Debug.Log("Added surface to graph");
+#endif
+                graph.InternalAddNavSurface(surface, record, proxyIndecies);
+            }
+        }
+
+        class RemoveNavSurfaceChange : IGraphChange
+        {
+            public NavGraphChange ChangeType => NavGraphChange.NavSurfaceRemoved;
+            public int ChangeSource { get; }
+
+            NavSurface surface;
+            GameObject changeSource;
+
+            public RemoveNavSurfaceChange(NavSurface surface)
+            {
+                this.surface = surface;
+                this.ChangeSource = surface.PBComponentId;
+            }
+
+            public void Apply(NavGraph graph)
+            {
+#if PBDEBUG
+                Debug.Log("Removed surface from graph");
+#endif
+                graph.InternalRemoveNavSurface(surface);
+            }
+        }
+
+        class AddNavLinkChange : IGraphChange
+        {
+            public NavGraphChange ChangeType => NavGraphChange.NavLinkAdded;
+            public int ChangeSource { get; }
+
+            INavLinkInstance link;
+            NavSegmentPositionPointer start;
+            NavSegmentPositionPointer goal;
+
+            public AddNavLinkChange(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal)
+            {
+                this.link = link;
+                this.start = start;
+                this.goal = goal;
+                ChangeSource = link.PBComponentId;
+            }
+
+            public void Apply(NavGraph graph)
+            {
+#if PBDEBUG
+                Debug.Log("Added link to graph");
+#endif
+                graph.InternalAddNavLink(link, start, goal);
+            }
+        }
+
+        class RemoveNavLinkChange : IGraphChange
+        {
+            public NavGraphChange ChangeType => NavGraphChange.NavLinkRemoved;
+            public int ChangeSource { get; }
+
+            INavLinkInstance link;
+            NavSegmentPositionPointer start;
+            NavSegmentPositionPointer goal;
+
+            public RemoveNavLinkChange(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal)
+            {
+                this.link = link;
+                this.start = start;
+                this.goal = goal;
+                ChangeSource = link.PBComponentId;
+            }
+
+            public void Apply(NavGraph graph)
+            {
+#if PBDEBUG
+                Debug.Log("Removed link from graph");
+#endif
+                graph.InternalRemoveNavLink(link, start, goal);
+            }
+        }
+
+        class MoveNavLinkStartChange : IGraphChange
+        {
+            public NavGraphChange ChangeType => NavGraphChange.NavLinkMoved;
+            public int ChangeSource { get; }
+
+            INavLinkInstance link;
+            NavSegmentPositionPointer start;
+            NavSegmentPositionPointer goal;
+            NavSegmentPositionPointer oldStart;
+
+            public MoveNavLinkStartChange(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal, NavSegmentPositionPointer oldStart)
+            {
+                this.link = link;
+                this.start = start;
+                this.goal = goal;
+                this.oldStart = oldStart;
+                ChangeSource = link.PBComponentId;
+            }
+
+            public void Apply(NavGraph graph)
+            {
+#if PBDEBUG
+                Debug.Log("Moved link start in graph");
+#endif
+                graph.InternalMoveNavLinkStart(link, start, goal, oldStart);
+            }
+        }
+
+        class MoveNavLinkGoalChange : IGraphChange
+        {
+            public NavGraphChange ChangeType => NavGraphChange.NavLinkMoved;
+            public int ChangeSource { get; }
+
+            INavLinkInstance link;
+            NavSegmentPositionPointer start;
+            NavSegmentPositionPointer goal;
+            NavSegmentPositionPointer oldGoal;
+
+            public MoveNavLinkGoalChange(INavLinkInstance link, NavSegmentPositionPointer start, NavSegmentPositionPointer goal, NavSegmentPositionPointer oldGoal)
+            {
+                this.link = link;
+                this.start = start;
+                this.goal = goal;
+                this.oldGoal = oldGoal;
+                ChangeSource = link.PBComponentId;
+            }
+
+            public void Apply(NavGraph graph)
+            {
+#if PBDEBUG
+                Debug.Log("Moved link goal in graph");
+#endif
+                graph.InternalMoveNavLinkGoal(link, start, goal, oldGoal);
+            }
+        }
+
+        class AddSegmentModifierChange : IGraphChange
+        {
+            public NavGraphChange ChangeType => NavGraphChange.SegmentModifierAdded;
+            public int ChangeSource { get; }
+
+            NavAreaMarkerInstance mod;
+
+            public AddSegmentModifierChange(NavAreaMarkerInstance mod)
+            {
+                this.mod = mod;
+                ChangeSource = mod.PBComponentId;
+            }
+
+            public void Apply(NavGraph graph)
+            {
+#if PBDEBUG
+                Debug.Log("Added modifier to graph");
+#endif
+                graph.InternalAddSegmentModifier(mod);
+            }
+        }
+
+        class RemoveSegmentModifierChange : IGraphChange
+        {
+            public NavGraphChange ChangeType => NavGraphChange.SegmentModifierRemoved;
+            public int ChangeSource { get; }
+
+            NavAreaMarkerInstance mod;
+
+            public RemoveSegmentModifierChange(NavAreaMarkerInstance mod)
+            {
+                this.mod = mod;
+                ChangeSource = mod.PBComponentId;
+            }
+
+            public void Apply(NavGraph graph)
+            {
+#if PBDEBUG
+                Debug.Log("Removed modifier to graph");
+#endif
+                graph.InternalRemoveSegmentModifier(mod);
+            }
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavGraph/NavGraph.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 026f93314fe2afa4db089d933acfcb9b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavGraph/NavGraphNode.cs

@@ -0,0 +1,96 @@
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    internal class NavGraphNode
+    {
+        public float LinkTargetT { get; set; }
+        public NavGraphNodeCluster LinkTarget { get; set; }
+
+        // location
+        public readonly NavGraphNodeCluster cluster;
+        public float t;
+
+        public readonly INavLinkInstance link;
+        public PathValues[] pathValues;
+
+        public bool IsGoal => link.LinkType == -1;
+
+        public NavGraphNode(int threadCount, NavGraphNodeCluster owner, float t, NavGraphNodeCluster linkTarget, float linkTargetT, INavLinkInstance original)
+        {
+            this.cluster = owner;
+            this.t = t;
+
+            this.LinkTargetT = linkTargetT;
+            this.LinkTarget = linkTarget;
+
+            this.link = original;
+
+            this.pathValues = new PathValues[threadCount];
+            InitializePathValueArray();
+        }
+
+        public NavGraphNode(int threadCount, NavGraphNodeCluster owner, float t)
+        {
+            this.cluster = owner;
+            this.t = t;
+            this.link = new ArtificialLink(-1);
+            this.LinkTarget = cluster;
+            this.LinkTargetT = t;
+
+            this.pathValues = new PathValues[threadCount];
+            InitializePathValueArray();
+        }
+
+        public IEnumerable<NavConnection> GetConnections(NavAgent agent, IList<NavGraphNode> goals, int pathValueId)
+        {
+            return LinkTarget.EnumerateReachableNavVerts(LinkTargetT, agent, goals, pathValueId);
+        }
+
+        // param must be local
+        public float HeuristicalCostsToGoal(Vector2 goal)
+        {
+            return Vector2.Distance(goal, GoalPosition());
+        }
+
+        public Vector2 GoalPosition()
+        {
+            return LinkTarget.GetPositionAlongSegment(LinkTargetT);
+        }
+
+        public Vector2 WGoalPosition()
+        { 
+            return LinkTarget.owner.LocalToWorld.MultiplyPoint3x4(LinkTarget.GetPositionAlongSegment(LinkTargetT));
+        }
+
+        public Vector2 Position() {
+            return cluster.GetPositionAlongSegment(t);
+        }
+
+        public Vector2 WPosition()
+        {
+            return cluster.owner.LocalToWorld.MultiplyPoint3x4(cluster.GetPositionAlongSegment(t));
+        }
+
+        private void InitializePathValueArray()
+        {
+            for (int i = 0; i < pathValues.Length; i++)
+            {
+                pathValues[i] = new PathValues(this);
+            }
+        }
+    }
+
+    internal struct NavConnection
+    {
+        public readonly NavGraphNode end;
+        public readonly float traversalCosts;
+
+        public NavConnection(NavGraphNode end, float traversalCosts)
+        {
+            this.end = end;
+            this.traversalCosts = traversalCosts;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavGraph/NavGraphNode.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: a5c4fab1500104040b48a3bb6aa7ba98
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavGraph/NavGraphNodeCluster.cs

@@ -0,0 +1,155 @@
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    internal class NavGraphNodeCluster : LineSegmentWithClearance
+    {
+        internal readonly List<NavGraphNode> nodes = new List<NavGraphNode>();
+        // unsorted
+        internal readonly List<NavAreaMarkerInstance> modifiers = new List<NavAreaMarkerInstance>(1);
+        internal readonly bool[] containsGoal;
+        public readonly NavSurfaceRecord owner;
+
+        public NavGraphNodeCluster(NavSegment segment, int threadCount, NavSurfaceRecord owner) : base(segment)
+        {
+            containsGoal = new bool[threadCount];
+            this.owner = owner;
+        }
+
+        public IEnumerable<NavConnection> EnumerateReachableNavVerts(float t, NavAgent agent, IList<NavGraphNode> goals, int pathValueId)
+        {
+            for (int iNode = 0; iNode < nodes.Count; iNode++)
+            {
+                var node = nodes[iNode];
+                if (!IsAgentAbleToTraverse(agent, t, node))
+                    continue;
+
+                yield return new NavConnection(node, node.link.TravelCosts(node.WPosition(), node.WGoalPosition()) * agent.GetLinkTraversalMultiplier(node.link.LinkType) * agent.GetNavTagTraversalMultiplier(node.link.NavTag) + TraversalCosts(t, node.t, agent));
+            }
+            if (containsGoal[pathValueId])
+            {
+                for (int iGoal = 0; iGoal < goals.Count; iGoal++)
+                {
+                    if (goals[iGoal].cluster == this && IsAgentAbleToTraverse(agent, t, goals[iGoal]))
+                        yield return new NavConnection(goals[iGoal], TraversalCosts(t, goals[iGoal].t, agent));
+                }
+            }
+        }
+
+        public void AddNode(int pathValueCount, float t, NavGraphNodeCluster linkTarget, float linkTargetT,
+            INavLinkInstance link)
+        {
+            nodes.Add(
+                new NavGraphNode(
+                    pathValueCount,
+                    this,
+                    t,
+                    linkTarget, linkTargetT,
+                    link
+                    ));
+        }
+
+        public void RemoveNode(INavLinkInstance link)
+        {
+            int index = nodes.FindIndex(node => node.link == link);
+            if (index != -1)
+                nodes.RemoveAt(index);
+        }
+
+        public void MoveNode(INavLinkInstance link, float newT)
+        {
+            int index = nodes.FindIndex(node => node.link == link);
+            nodes[index].t = newT;
+        }
+
+        public NavGraphNode GetNode(INavLinkInstance link)
+        {
+            return nodes.Find(node => node.link == link);
+        }
+
+        public bool DoesNodeExist(INavLinkInstance link)
+        {
+            return nodes.Exists(node => node.link == link);
+        }
+
+        public void ApplyCellClearances(float[] differentClearances)
+        {
+            for (int i = 0; i < cellClearances.Length; i++)
+            {
+                cellClearances[i] = Mathf.Min(cellClearances[i], differentClearances[i]);
+            }
+        }
+
+        public void AddNodeClusterModifier(NavAreaMarkerInstance mod)
+        {
+            modifiers.Add(mod);
+        }
+
+        public void RemoveNodeClusterModifier(NavAreaMarkerInstance mod)
+        {
+            modifiers.Remove(mod);
+        }
+
+        public int GetNavTagVector(float pos)
+        {
+            int vector = 0;
+            foreach (var mod in modifiers)
+            {
+                if (mod.T <= pos && mod.T + mod.Length >= pos)
+                    vector |= (1 << mod.NavTag);
+            }
+            return vector;
+        }
+
+        public int GetNavTagVector(Vector2 pos)
+        {
+            return GetNavTagVector(DistanceOfPointAlongSegment(pos));
+        }
+
+        public bool CanAgentReachPoint(NavAgent agent, float startT, float goalT)
+        {
+            return agent.CanTraverseSegment(owner.LocalToWorld.MultiplyVector(Normal), GetMinClearanceTo(startT, goalT));
+        }
+
+        public bool CanAgentBeAtPoint(NavAgent agent, float t)
+        {
+            return Vector2.Angle(Vector2.up, owner.LocalToWorld.MultiplyVector(Normal)) <= agent.MaxSlopeAngle && GetClearanceAlongSegment(t) >= agent.Height && (GetNavTagVector(t) & ~agent.NavTagMask) == 0;
+        }
+
+        private bool IsAgentAbleToTraverse(NavAgent agent, float startT, NavGraphNode node)
+        {
+            return agent.CanTraverseLink(node.link) && CanAgentReachPoint(agent, startT, node.t);
+        }
+
+        private float TraversalCosts(float t, float goal, NavAgent agent)
+        {
+            float a, b;
+            if (t < goal)
+            {
+                a = t;
+                b = goal;
+            }
+            else
+            {
+                a = goal;
+                b = t;
+            }
+            if (modifiers.Count == 0)
+            {
+                return b - a;
+            }
+
+            float costs = b - a;
+            foreach (var mod in modifiers)
+            {
+                if (mod.T + mod.Length <= a || mod.T >= b)
+                    continue;
+
+                // some overlap exists
+                costs += (Mathf.Min(mod.T + mod.Length, b) - Mathf.Max(mod.T, a)) * agent.GetNavTagTraversalMultiplier(mod.NavTag);
+            }
+            return costs;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavGraph/NavGraphNodeCluster.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 09696d0a858d6a249aac9a2ffc71fd9f
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavGraph/NavSurfaceRecord.cs

@@ -0,0 +1,49 @@
+using System.Collections.Generic;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    internal class NavSurfaceRecord
+    {
+        public B2DynamicTree<NavGraphNodeCluster> Clusters { get; private set; }
+        public List<INavLinkInstance> SoftRefLinks { get; private set; }
+
+        public Matrix4x4 LocalToWorld { get => localToWorld; set {
+                localToWorld = value;
+                worldToLocal = localToWorld.inverse;
+            } }
+        public Matrix4x4 WorldToLocal => worldToLocal;
+        public readonly NavSurface navSurface;
+        public readonly int bakeIteration;
+
+        private Matrix4x4 localToWorld;
+        private Matrix4x4 worldToLocal;
+
+        public NavSurfaceRecord(B2DynamicTree<NavGraphNodeCluster> tree, Matrix4x4 localToWorld, NavSurface navSurface)
+        {
+            this.Clusters = tree;
+            SoftRefLinks = new List<INavLinkInstance>();
+            this.localToWorld = localToWorld;
+            this.navSurface = navSurface;
+            this.bakeIteration = navSurface.BakeIteration;
+        }
+
+        public void AddSoftRefLink(INavLinkInstance instance)
+        {
+            SoftRefLinks.Add(instance);
+        }
+
+        public void RemoveSoftRefLink(INavLinkInstance instance)
+        {
+            SoftRefLinks.Remove(instance);
+        }
+
+        public void Destroy(NavGraph graph)
+        {
+            foreach (var ls in SoftRefLinks)
+            {
+                ls.OnRemove();
+            }
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavGraph/NavSurfaceRecord.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 6641bbd92fce18c489cab9aaaee6d4f4
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 9c37197ee5c1fb446ac98c67d0aa09bd
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/ArtificialLink.cs

@@ -0,0 +1,41 @@
+using System;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    internal class ArtificialLink : INavLinkInstance
+    {
+        public NavSegmentPositionPointer Start => throw new NotImplementedException();
+
+        public NavSegmentPositionPointer Goal => throw new NotImplementedException();
+
+        public int LinkType => linkType;
+
+        public string LinkTypeName => throw new NotImplementedException();
+
+        public GameObject GameObject => throw new NotImplementedException();
+
+        public float Clearance => float.MaxValue;
+
+        public int NavTag => 0;
+
+        public bool IsTraversable => true;
+        public int PBComponentId => 0;
+
+        private int linkType;
+
+        public ArtificialLink(int linkType)
+        {
+            this.linkType = linkType;
+        }
+
+        public float TravelCosts(Vector2 start, Vector2 goal)
+        {
+            return 0;
+        }
+
+        public void OnRemove()
+        {
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/ArtificialLink.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 37b5756b029e50e409aec9ab1f691b6b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/BaseNavLink.cs

@@ -0,0 +1,109 @@
+using System;
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Common basis for NavLink and NavLinkCluster.
+    /// </summary>
+    public abstract class BaseNavLink : MonoBehaviour, INavLinkInstanceCreator
+    {
+        public int LinkType
+        {
+            get { return linkType; }
+            set {
+                if (value < 0 || value >= PathBerserker2dSettings.NavLinkTypeNames.Length)
+                    throw new ArgumentOutOfRangeException($"{value} is not a valid link type.");
+                linkType = value;
+            }
+        }
+        public string LinkTypeName
+        {
+            get { return PathBerserker2dSettings.NavLinkTypeNames[linkType]; }
+            set { linkType = PathBerserker2dSettings.GetLinkTypeFromName(value); }
+        }
+        public float Clearance
+        {
+            get { return clearance; }
+            set { clearance = value; }
+        }
+        public float AvgWaitTime
+        {
+            get { return avgWaitTime; }
+            set { avgWaitTime = value; }
+        }
+        public float CostOverride
+        {
+            get { return costOverride; }
+            set { costOverride = value; }
+        }
+        public GameObject GameObject => gameObject;
+        public int NavTag
+        {
+            get { return navTag; }
+            set { navTag = PathBerserker2dSettings.EnsureNavTagExists(value); }
+        }
+        public float MaxTraversableDistance
+        {
+            get { return maxTraversableDistance; }
+            set { maxTraversableDistance = value; }
+        }
+
+        public int PBComponentId { get; protected set; }
+
+        [Tooltip("Cost of traversing this link. If this is <= 0 the distance between start and goal is used instead.")]
+        [SerializeField]
+        protected float costOverride = -1;
+
+        [SerializeField]
+        protected int linkType = 1;
+
+        [Tooltip("Maximum height an agent can be to traverse this link.")]
+        [SerializeField]
+        protected float clearance = 2;
+
+        [SerializeField]
+        protected int navTag = 0;
+
+        [Tooltip("Average time an agent has to wait before starting to traverse this link. This is purely to tune the pathfinding algorithm.")]
+        [SerializeField]
+        protected float avgWaitTime = 0;
+
+        [Tooltip("Maximum distances between start and goal, that is considered traversable. If this distance is exceeded (e.g. on a moving platform) an agent will wait to traverse this link.")]
+        [SerializeField]
+        protected float maxTraversableDistance = 0;
+
+        /// <summary>
+        /// Should this link be automatically mapped. If not, you have to call UpdateMapping() yourself.
+        /// </summary>
+        [SerializeField, Tooltip("Should this link be automatically mapped. If not, you have to call UpdateMapping() yourself.")]
+        public bool autoMap = true;
+
+        protected virtual void Awake()
+        {
+            PBComponentId = PBWorld.GeneratePBComponentId();
+        }
+
+        protected virtual void OnValidate()
+        {
+            linkType = PathBerserker2dSettings.EnsureNavLinkTypeExists(linkType);
+            navTag = PathBerserker2dSettings.EnsureNavTagExists(navTag);
+        }
+
+        /// <summary>
+        /// MUST BE THREAD SAFE!
+        /// Calculates the cost of traversing from start to goal.
+        /// </summary>
+        /// <param name="start"></param>
+        /// <param name="goal"></param>
+        /// <returns></returns>
+        public float TravelCosts(Vector2 start, Vector2 goal)
+        {
+            float costOverride = this.costOverride;
+            if (costOverride >= 0)
+                return costOverride + avgWaitTime;
+            else
+                return Mathf.Max(maxTraversableDistance, Vector2.Distance(start, goal)) + avgWaitTime;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/BaseNavLink.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: ec2706faa1499754ab7972b2ce01acf5
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/DynamicObstacle.cs

@@ -0,0 +1,13 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Attach to GameObject to mark it as dynamic.
+    /// Dynamic objects are ignored while baking a NavSurface.
+    /// </summary>
+    public class DynamicObstacle : MonoBehaviour
+    {
+        // ignored for baking
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/DynamicObstacle.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 098039b2e353c6242b6b156886f24db4
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/INavLink.cs

@@ -0,0 +1,31 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Internal instance of a NavLink.
+    /// </summary>
+    /// <remarks>
+    /// An instance is deliberately kept very basic. It links from a given start to a given goal.
+    /// </remarks>
+    public interface INavLinkInstance
+    {
+        //NavSegmentPositionPointer Start { get; }
+        //NavSegmentPositionPointer Goal { get; }
+        int LinkType { get; }
+        int NavTag { get; }
+
+        bool IsTraversable { get; }
+
+        string LinkTypeName { get; }
+
+        GameObject GameObject { get; }
+
+        float Clearance { get; }
+        int PBComponentId { get; }
+
+        float TravelCosts(Vector2 start, Vector2 goal);
+
+        void OnRemove();
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/INavLink.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: cb4c816f3b23aac42a66b17253265a5a
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/INavLinkInstanceCreator.cs

@@ -0,0 +1,19 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    internal interface INavLinkInstanceCreator
+    {
+        int LinkType { get; }
+        int NavTag { get; }
+        int PBComponentId { get; }
+
+        GameObject GameObject { get; }
+        float Clearance { get; }
+        float TravelCosts(Vector2 start, Vector2 goal);
+        float CostOverride { get; }
+
+        float AvgWaitTime { get; }
+        float MaxTraversableDistance { get; }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/INavLinkInstanceCreator.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: c7dfb46afa8de8f4fb172b5323f315e9
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/NavAreaMarker.cs

@@ -0,0 +1,173 @@
+using UnityEngine;
+using System.Collections.Generic;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Marks all segments within an area with a specific NavTag.
+    /// </summary>
+    [RequireComponent(typeof(RectTransform))]
+    [AddComponentMenu("PathBerserker2d/Nav Area Marker")]
+    [HelpURL("https://oribow.github.io/PathBerserker2dDemo/Documentation/classPathBerserker2d_1_1NavAreaMarker.html")]
+    public class NavAreaMarker : MonoBehaviour
+    {
+        public int NavTag
+        {
+            get => navTag;
+            set
+            {
+                navTag = PathBerserker2dSettings.EnsureNavTagExists(value);
+            }
+        }
+        public Color MarkerColor => PathBerserker2dSettings.GetNavTagColor(navTag);
+
+        [SerializeField]
+        int navTag = 0;
+
+        [Tooltip("Minimum angle between the segment tangent and up. Use this to only mark segments with certain angles.")]
+        [SerializeField, Range(0, 360)]
+        float minAngle = 0;
+
+        [Tooltip("Maximum angle between the segment tangent and up. Use this to only mark segments with certain angles.")]
+        [SerializeField, Range(0, 360)]
+        float maxAngle = 360;
+
+        /// <summary>
+        /// Updates the modified marked area after a continuous time period of no movement.
+        /// </summary>
+        [Tooltip("Updates the modified marked area after a continuous time period of no movement.")]
+        [SerializeField]
+        public float updateAfterTimeOfNoMovement = 0.2f;
+
+        /// <summary>
+        /// Updates the modified marked area after this amount of time passed.
+        /// </summary>
+        [Tooltip("Updates the modified marked area after this amount of time passed.")]
+        [SerializeField]
+        public float updateAfterTime = 1;
+
+        public int PBComponentId { get; }
+
+        private RectTransform rectTransform;
+        private List<NavAreaMarkerInstance> instances;
+        private float lastMovementTime;
+        private float isDirtySince;
+        private bool isDirty = false;
+
+        #region UNITY
+        private void OnEnable()
+        {
+            rectTransform = GetComponent<RectTransform>();
+            instances = new List<NavAreaMarkerInstance>();
+            transform.hasChanged = false;
+            AddToGraph();
+        }
+
+        private void OnDisable()
+        {
+            RemoveFromGraph();
+        }
+
+        private void Update()
+        {
+            if (transform.hasChanged)
+            {
+                if (!isDirty)
+                    isDirtySince = Time.time;
+
+                isDirty = true;
+                transform.hasChanged = false;
+                lastMovementTime = Time.time;
+            }
+
+            if (isDirty && 
+                (Time.time - lastMovementTime > updateAfterTimeOfNoMovement
+                || Time.time - isDirtySince > updateAfterTime))
+            {
+                AddToGraph();
+                lastMovementTime = float.MaxValue;
+            }
+        }
+
+        private void OnValidate()
+        {
+            navTag = PathBerserker2dSettings.EnsureNavTagExists(navTag);
+            updateAfterTimeOfNoMovement = Mathf.Max(0, updateAfterTimeOfNoMovement);
+            updateAfterTime = Mathf.Max(0, updateAfterTime);
+        }
+
+        private void Reset()
+        {
+            // only modify sizeDelta, if its at default value
+            var rt = GetComponent<RectTransform>();
+            if (rt.sizeDelta == new Vector2(100, 100))
+                rt.sizeDelta = Vector2.one;
+        }
+        #endregion
+
+        /// <summary>
+        /// Updates area of effect mapping. Call after modifying NavAreaMarker transform.
+        /// Alternatively, instead of calling this function, set transform.hasChanged to true.
+        /// </summary>
+        public void UpdateMappings()
+        {
+            AddToGraph();
+            lastMovementTime = float.MaxValue;
+        }
+
+        private void AddToGraph()
+        {
+            isDirty = false;
+            if (instances.Count > 0)
+                RemoveFromGraph();
+
+            var r = rectTransform.rect;
+            Vector2 scaleFactor = rectTransform.lossyScale * r.size * 0.5f;
+            Vector2 center = r.center;
+
+            r.min = center - scaleFactor + (Vector2)rectTransform.position;
+            r.max = center + scaleFactor + (Vector2)rectTransform.position;
+
+            var results = PBWorld.BoxCastWithStaged(r, rectTransform.rotation.eulerAngles.z, minAngle, maxAngle);
+
+            foreach (var pointer in results)
+            {
+                var instance = new NavAreaMarkerInstance(this, pointer);
+                PBWorld.NavGraph.AddSegmentModifier(instance);
+                instances.Add(instance);
+            }
+        }
+
+        private void RemoveFromGraph()
+        {
+            foreach (var instance in instances)
+            {
+                PBWorld.NavGraph.RemoveSegmentModifier(instance);
+            }
+            instances.Clear();
+        }
+    }
+
+    internal class NavAreaMarkerInstance
+    {
+        public int NavTag => original.NavTag;
+        public float T => position.t;
+        public float Length => position.length;
+
+        public NavSubsegmentPointer position;
+        public int PBComponentId => original.PBComponentId;
+
+        NavAreaMarker original;
+
+        public NavAreaMarkerInstance(NavAreaMarker original)
+        {
+            this.original = original;
+        }
+
+        public NavAreaMarkerInstance(NavAreaMarker original, NavSubsegmentPointer position)
+        {
+            this.original = original;
+            this.position = position;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/NavAreaMarker.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: cd188745fa9d851459105e6651b2b80a
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/NavLink.cs

@@ -0,0 +1,207 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// A link from one segment to another.
+    /// </summary>
+    /// <remarks>
+    /// NavLink gets added to the pathfinder at runtime.
+    /// It can be loaded and unloaded by enabling / disabling the component
+    /// After being loaded and added to the pathfinder, the position of the link will not be updated.
+    /// For example that means, if your link start position gets mapped to a position on a moving platform, the initial mapping of the link start to the segment won't change.
+    /// The mapped position is relative to the NavSurface containing the moving platform. 
+    /// It will follow the movements of the platform, even though the start marker of the link will not.
+    /// ## Mapping
+    /// You can update the links mapping by calling <see cref="UpdateMapping"/> 
+    ///
+    /// Internally, when moving the start and end marker around in scene view while the game is playing,  <see cref="UpdateMapping"/> is called.
+    /// That means you can move the markers around and the link will update its mapping.
+    /// If you move them around by any other means however, you have to call <see cref="UpdateMapping"/> afterwards.
+    /// ## Visualization
+    /// Everything visualization related is purely for you and is not meant to accessed at runtime.
+    /// Visualizations like bezier or projectile are meant to allow you to figure out a good clearance value.
+    /// ## Traversable
+    /// When a link is marked bidirectional, internally two links get added to the pathfinder. One for each direction.
+    /// Both links traversable can be set separately with <see cref="SetStartToGoalLinkTraversable"/> and <see cref="SetGoalToStartLinkTraversable"/>.
+    ///
+    /// **Not being traversable should always be temporary.** In the sense of, **"this link is not traversable right now, but will be in the future"**.
+    /// NavAgents will wait indefinitely for the link to become traversable again.
+    /// You can adjust AvgWaitTime to increase the cost of such not alway traversable links.
+    /// The pathfinder will add it to the cost of traversal.
+    /// The pathfinder does not care if a link is marked as traversable or not. It only cares about the cost of traversal.
+    /// If you want to disable a link for a longer time, consider disabling the link component. Then it will be unloaded and not considered for any pathfinding.
+    /// </remarks>
+    [AddComponentMenu("PathBerserker2d/Nav Link")]
+    public sealed class NavLink : BaseNavLink
+    {
+        public enum VisualizationType
+        {
+            Linear = 0,
+            QuadradticBezier = 1,
+            Projectile = 2,
+            Teleport = 3,
+            None = 4,
+            TransformBasedMovement = 5
+        }
+
+        public Vector2 GoalWorldPosition
+        {
+            get
+            {
+                return transform.TransformPoint(goal);
+            }
+            set
+            {
+                goal = transform.InverseTransformPoint(value);
+            }
+        }
+        public Vector2 StartWorldPosition
+        {
+            get
+            {
+                return transform.TransformPoint(start);
+            }
+            set
+            {
+                start = transform.InverseTransformPoint(value);
+            }
+        }
+        public Vector2 StartLocalPosition
+        {
+            get
+            {
+                return start;
+            }
+            set
+            {
+                start = value;
+            }
+        }
+        public Vector2 GoalLocalPosition
+        {
+            get
+            {
+                return goal;
+            }
+            set
+            {
+                goal = value;
+            }
+        }
+
+        public VisualizationType CurrentVisualizationType { get { return visualizationType; } }
+
+        public bool IsBidirectional
+        {
+            get { return isBidirectional; }
+            set
+            {
+                if (isBidirectional != value && !value)
+                {
+                    linkGoalToStart.RemoveFromWorld();
+                }
+                isBidirectional = value;
+            }
+        }
+
+        public bool IsAddedToWorld => linkStartToGoal?.IsAdded ?? false;
+
+        internal float HorizontalSpeed => horizontalSpeed;
+        internal float TraversalAngle => traversalAngle;
+        internal Vector2 BezierControlPoint { get => bezierControlPoint; set => bezierControlPoint = value; }
+
+        [Header("Location")]
+        [SerializeField]
+        Vector2 start = Vector2.left * 2;
+        [SerializeField]
+        Vector2 goal = Vector2.right * 2;
+        [SerializeField]
+        bool isBidirectional = true;
+        [SerializeField]
+        VisualizationType visualizationType = VisualizationType.TransformBasedMovement;
+        [SerializeField]
+        float traversalAngle = 0;
+        [SerializeField]
+        float horizontalSpeed = 1;
+        [SerializeField, HideInInspector]
+        Vector2 bezierControlPoint = Vector2.up * 3;
+
+        private NavLinkInstance linkStartToGoal;
+        private NavLinkInstance linkGoalToStart;
+
+        #region UNITY
+        private void OnEnable()
+        {
+            if (linkStartToGoal == null)
+                linkStartToGoal = new NavLinkInstance(this);
+            if (linkGoalToStart == null)
+                linkGoalToStart = new NavLinkInstance(this);
+
+            AutoUpdateMapping();
+        }
+
+        private void OnDisable()
+        {
+            linkStartToGoal.RemoveFromWorld();
+            linkGoalToStart.RemoveFromWorld();
+        }
+
+        protected override void OnValidate()
+        {
+            base.OnValidate();
+            if (linkGoalToStart != null && (!linkGoalToStart.IsAdded || linkStartToGoal != null))
+            {
+                AutoUpdateMapping();
+
+                if (!isBidirectional)
+                    linkGoalToStart.RemoveFromWorld();
+            }
+        }
+        #endregion
+
+        /// <summary>
+        /// Update the mapping for both link instances. Call after link positions have been changed. 
+        /// </summary>
+        public void UpdateMapping()
+        {
+            NavSegmentPositionPointer navStart, navGoal;
+            if (PBWorld.TryMapPointWithStaged(StartWorldPosition, out navStart)
+                && PBWorld.TryMapPointWithStaged(GoalWorldPosition, out navGoal))
+            {
+                linkStartToGoal.UpdateMapping(navStart, navGoal, StartWorldPosition, GoalWorldPosition);
+                linkGoalToStart.UpdateMapping(navGoal, navStart, GoalWorldPosition, StartWorldPosition);
+
+                linkStartToGoal.AddToWorld();
+                if (isBidirectional) linkGoalToStart.AddToWorld();
+            }
+            else
+            {
+                linkStartToGoal.RemoveFromWorld();
+                linkGoalToStart.RemoveFromWorld();
+            }
+        }
+
+        /// <summary>
+        /// Set the link instance from start point to goal traversable.
+        /// </summary>
+        public void SetStartToGoalLinkTraversable(bool traversable)
+        {
+            this.linkStartToGoal.IsTraversable = traversable;
+        }
+
+        /// <summary>
+        /// Set the link instance from goal point to start traversable. This link only exist, if the link is bidirectional.
+        /// </summary>
+        public void SetGoalToStartLinkTraversable(bool traversable)
+        {
+            this.linkGoalToStart.IsTraversable = traversable;
+        }
+
+        private void AutoUpdateMapping()
+        {
+            if (autoMap)
+                UpdateMapping();
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/NavLink.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 87d47b3e0cb42914b8b2ae885bebf30b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {fileID: 2800000, guid: 579b19e3fcbb57e43adcf1e3f754f5ef, type: 3}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/NavLinkCluster.cs

@@ -0,0 +1,118 @@
+using UnityEngine;
+using System.Collections.Generic;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Creates links to interconnect a collection.
+    /// </summary>
+    /// <remarks>
+    /// Consists of a list of points. 
+    /// At runtime a link is generated for each point to connect it with each other point.
+    /// This is a convenience component. It drastically reduces the amount of work required to setup an elevator or ladder for example.
+    ///
+    /// Otherwise it functions and behaves the same as a NavLink.
+    /// Reference the documentation for NavLink for further details.
+    /// </remarks>
+    [AddComponentMenu("PathBerserker2d/Nav Link Cluster")]
+    public sealed class NavLinkCluster : BaseNavLink
+    {
+        internal enum PointTraversalType
+        {
+            Exit,
+            Entry,
+            Both
+        }
+
+        internal LinkPoint[] LinkPoints => linkPoints;
+
+        [SerializeField]
+        internal LinkPoint[] linkPoints = new LinkPoint[] { new LinkPoint(Vector2.left * 2), new LinkPoint(Vector2.right * 2) };
+
+        private List<NavLinkInstance> linkInstances;
+
+        #region UNITY
+        private void OnEnable()
+        {
+            if (linkInstances == null)
+                linkInstances = new List<NavLinkInstance>();
+
+            if (autoMap)
+                UpdateMapping();
+        }
+
+        private void OnDisable()
+        {
+            foreach (var li in linkInstances)
+                li.RemoveFromWorld();
+        }
+        #endregion
+
+        /// <summary>
+        /// Update the mapping for all link instances. Call after link positions have been changed.
+        /// </summary>
+        public void UpdateMapping()
+        {
+            NavSegmentPositionPointer navStart, navGoal;
+            int instanceCounter = 0;
+            foreach (var startPoint in linkPoints)
+            {
+                if (startPoint.traversalType == PointTraversalType.Exit)
+                    continue;
+
+                Vector2 worldStart = transform.TransformPoint(startPoint.point);
+                foreach (var goalPoint in linkPoints)
+                {
+                    if (goalPoint.traversalType == PointTraversalType.Entry || goalPoint.point == startPoint.point)
+                        continue;
+
+                    Vector2 worldGoal = transform.TransformPoint(goalPoint.point);
+                    if (instanceCounter >= linkInstances.Count)
+                    {
+                        linkInstances.Add(new NavLinkInstance(this));
+                    }
+                    var linkInstance = linkInstances[instanceCounter++];
+
+                    if (PBWorld.TryMapPointWithStaged(worldStart, out navStart)
+                        && PBWorld.TryMapPointWithStaged(worldGoal, out navGoal))
+                    {
+                        linkInstance.UpdateMapping(navStart, navGoal, worldStart, worldGoal);
+                        linkInstance.AddToWorld();
+                    }
+                    else
+                    {
+                        linkInstance.RemoveFromWorld();
+                    }
+                }
+            }
+        }
+
+        /// <summary>
+        /// Set link instances to be traversable based on their start and end points.
+        /// </summary>
+        /// <param name="traversableFunc">Determines whether to enable or disable the given link instance. Link instance is given as its start and goal position.</param>
+        public void SetLinksTraversable(System.Func<Vector2, Vector2, bool> traversableFunc)
+        {
+            foreach (var link in linkInstances)
+            {
+                if (link.IsAdded)
+                    link.IsTraversable = traversableFunc(link.Start.Position, link.Goal.Position);
+            }
+        }
+
+        [System.Serializable]
+        internal struct LinkPoint
+        {
+            [SerializeField]
+            public Vector2 point;
+            [SerializeField]
+            public PointTraversalType traversalType;
+
+            public LinkPoint(Vector2 point)
+            {
+                this.point = point;
+                this.traversalType = PointTraversalType.Both;
+            }
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/NavLinkCluster.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: be2a77a7bf2e29d438041e3d46dd5667
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/NavLinkInstance.cs

@@ -0,0 +1,106 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    internal sealed class NavLinkInstance : INavLinkInstance
+    {
+        public NavSegmentPositionPointer Start => start;
+        public NavSegmentPositionPointer Goal => goal;
+        public int LinkType => creator.LinkType;
+        public string LinkTypeName => PathBerserker2dSettings.GetLinkTypeName(creator.LinkType);
+        public GameObject GameObject => creator.GameObject;
+        public bool IsAdded => isAdded;
+        public float Clearance => creator.Clearance;
+        public float CostOverride => creator.CostOverride;
+        public int NavTag => creator.NavTag;
+        public bool IsTraversable
+        {
+            get
+            {
+                return isTraversable && (creator.MaxTraversableDistance <= 0 || Vector2.Distance(Start.Position, Goal.Position) <= creator.MaxTraversableDistance);
+            }
+            set
+            {
+                isTraversable = value;
+            }
+        }
+        public int PBComponentId => creator.PBComponentId;
+        internal INavLinkInstanceCreator Creator => creator;
+
+
+        private INavLinkInstanceCreator creator;
+        private NavSegmentPositionPointer start;
+        private Vector2 startPos;
+        private NavSegmentPositionPointer goal;
+        private Vector2 goalPos;
+        private bool isAdded = false;
+        private bool isTraversable = true;
+
+        public NavLinkInstance(INavLinkInstanceCreator creator)
+        {
+            this.creator = creator;
+        }
+
+        public float TravelCosts(Vector2 start, Vector2 goal)
+        {
+            return creator.TravelCosts(start, goal);
+        }
+
+
+
+        public void AddToWorld()
+        {
+            if (!isAdded)
+            {
+                PBWorld.NavGraph.AddNavLink(this, start, goal);
+                isAdded = true;
+            }
+        }
+
+        public void RemoveFromWorld()
+        {
+            if (isAdded)
+            {
+                PBWorld.NavGraph.RemoveNavLink(this, start, goal);
+                isAdded = false;
+            }
+        }
+
+        public void UpdateMapping(NavSegmentPositionPointer start, NavSegmentPositionPointer goal, Vector2 startPos, Vector2 goalPos)
+        {
+            this.startPos = startPos;
+            this.goalPos = goalPos;
+
+            if (isAdded)
+            {
+                if (start != this.start)
+                {
+                    var oldPos = this.start;
+                    this.start = start;
+
+                    PBWorld.NavGraph.MoveNavLinkStart(this, start, goal, oldPos);
+                }
+                if (goal != this.goal)
+                {
+                    var oldPos = this.goal;
+                    this.goal = goal;
+
+                    PBWorld.NavGraph.MoveNavLinkGoal(this, start, goal, oldPos);
+                }
+            }
+            else
+            {
+                this.start = start;
+                this.goal = goal;
+            }
+        }
+
+        public void OnRemove()
+        {
+            if (start.surface != null)
+                RemoveFromWorld();
+            else
+                isAdded = false;
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/NavLinkInstance.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: b8d40862c6107ef4a856664b359e51b9
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/NavSegmentSubstractor.cs

@@ -0,0 +1,25 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    /// <summary>
+    /// Marks with its RectTransform an area to remove segments from.
+    /// </summary>
+    [RequireComponent(typeof(RectTransform))]
+    public class NavSegmentSubstractor : MonoBehaviour
+    {
+        /// <summary>
+        /// Minimum angle between the segment tangent and up. Use this to only remove segments with certain angles.
+        /// </summary>
+        [Tooltip("Minimum angle between the segment tangent and up. Use this to only remove segments with certain angles.")]
+        [SerializeField, Range(0, 360)]
+        public float fromAngle = 0;
+
+        /// <summary>
+        /// Maximum angle between the segment tangent and up. Use this to only remove segments with certain angles.
+        /// </summary>
+        [Tooltip("Maximum angle between the segment tangent and up. Use this to only remove segments with certain angles.")]
+        [SerializeField, Range(0, 360)]
+        public float toAngle = 360;
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/NavSegmentSubstractor.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: a276efee5bef16c408d4567ee5ed9369
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/SegmentConnectionLink.cs

@@ -0,0 +1,45 @@
+using UnityEngine;
+
+namespace PathBerserker2d
+{
+    internal class CornerLink : INavLinkInstance
+    {
+        public NavSegmentPositionPointer Start => start;
+
+        public NavSegmentPositionPointer Goal => goal;
+
+        public int LinkType => 0;
+
+        public string LinkTypeName => PathBerserker2dSettings.GetLinkTypeName(0);
+
+        public GameObject GameObject => null;
+
+        public float Clearance => float.MaxValue;
+
+        public int NavTag => 0;
+
+        public bool IsTraversable => true;
+        public int PBComponentId => 0;
+
+        private NavSegmentPositionPointer start;
+        private NavSegmentPositionPointer goal;
+        private float angle;
+
+        public CornerLink(NavSegmentPositionPointer start, NavSegmentPositionPointer goal, float angle)
+        {
+            this.start = start;
+            this.goal = goal;
+            this.angle = angle;
+        }
+
+        public float TravelCosts(Vector2 start, Vector2 goal)
+        {
+            return angle;
+        }
+
+        public void OnRemove()
+        {
+
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavObjects/SegmentConnectionLink.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: e3c7717cb27cab34d9ec825e3cbc10ea
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavSurface.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 874b67fb230896a43a6d44a87da38828
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavSurface/ColliderLayerFilter.cs

@@ -0,0 +1,26 @@
+using UnityEngine;
+using System.Collections.Generic;
+using System.Linq;
+
+namespace PathBerserker2d
+{
+    internal class ColliderLayerFilter : IColliderFilter
+    {
+        private LayerMask layerMask;
+        private bool onlyStatic;
+
+        public ColliderLayerFilter(LayerMask layerMask, bool onlyStatic)
+        {
+            this.layerMask = layerMask;
+            this.onlyStatic = onlyStatic;
+        }
+
+        public IEnumerable<Collider2D> Filter(IEnumerable<Collider2D> colliders)
+        {
+            return colliders.Where(
+                (col) => layerMask.IsLayerWithinMask(col.gameObject.layer) &&
+                !col.GetComponent<DynamicObstacle>() &&
+                (!onlyStatic || col.gameObject.isStatic));
+        }
+    }
+}

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavSurface/ColliderLayerFilter.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 548bcd56f78f38e418eb0a3ec41825a8
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/PathBerserker2d/Scripts/PathBerserker2d/NavSurface/Creation.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 8083133de4af48e48b87fdb05b356ca2
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: