1 files changed, 247 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs
new file mode 100644
index 0000000..ad14f12
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Utilities/GraphGizmoHelper.cs
@@ -0,0 +1,247 @@
+using UnityEngine;
+
+namespace Pathfinding.Util {
+	using Pathfinding.Drawing;
+
+	/// <summary>Combines hashes into a single hash value</summary>
+	public struct NodeHasher {
+		readonly bool includePathSearchInfo;
+		readonly bool includeAreaInfo;
+		readonly bool includeHierarchicalNodeInfo;
+		readonly PathHandler debugData;
+		public DrawingData.Hasher hasher;
+
+		public NodeHasher(AstarPath active) {
+			hasher = default;
+			this.debugData = active.debugPathData;
+			includePathSearchInfo = debugData != null && (active.debugMode == GraphDebugMode.F || active.debugMode == GraphDebugMode.G || active.debugMode == GraphDebugMode.H || active.showSearchTree);
+			includeAreaInfo = active.debugMode == GraphDebugMode.Areas;
+			includeHierarchicalNodeInfo = active.debugMode == GraphDebugMode.HierarchicalNode;
+			hasher.Add(active.debugMode);
+			hasher.Add(active.debugFloor);
+			hasher.Add(active.debugRoof);
+			hasher.Add(AstarColor.ColorHash());
+		}
+
+		public void HashNode (GraphNode node) {
+			hasher.Add(node.GetGizmoHashCode());
+			if (includeAreaInfo) hasher.Add((int)node.Area);
+			if (includeHierarchicalNodeInfo) hasher.Add(node.HierarchicalNodeIndex);
+
+			if (includePathSearchInfo) {
+				var pathNode = debugData.pathNodes[node.NodeIndex];
+				hasher.Add(pathNode.pathID);
+				hasher.Add(pathNode.pathID == debugData.PathID);
+				// hasher.Add(pathNode.F);
+			}
+		}
+
+		public void Add<T>(T v) {
+			hasher.Add(v);
+		}
+
+		public static implicit operator DrawingData.Hasher(NodeHasher hasher) {
+			return hasher.hasher;
+		}
+	}
+
+	public class GraphGizmoHelper : IAstarPooledObject, System.IDisposable {
+		public DrawingData.Hasher hasher { get; private set; }
+		PathHandler debugData;
+		ushort debugPathID;
+		GraphDebugMode debugMode;
+		bool showSearchTree;
+		float debugFloor;
+		float debugRoof;
+		public CommandBuilder builder;
+		Vector3 drawConnectionStart;
+		Color drawConnectionColor;
+		readonly System.Action<GraphNode> drawConnection;
+#if UNITY_EDITOR
+		UnsafeSpan<GlobalNodeStorage.DebugPathNode> debugPathNodes;
+#endif
+		GlobalNodeStorage nodeStorage;
+
+		public GraphGizmoHelper () {
+			// Cache a delegate to avoid allocating memory for it every time
+			drawConnection = DrawConnection;
+		}
+
+		public static GraphGizmoHelper GetSingleFrameGizmoHelper (DrawingData gizmos, AstarPath active, RedrawScope redrawScope) {
+			return GetGizmoHelper(gizmos, active, DrawingData.Hasher.NotSupplied, redrawScope);
+		}
+
+		public static GraphGizmoHelper GetGizmoHelper (DrawingData gizmos, AstarPath active, DrawingData.Hasher hasher, RedrawScope redrawScope) {
+			var helper = ObjectPool<GraphGizmoHelper>.Claim();
+
+			helper.Init(active, hasher, gizmos, redrawScope);
+			return helper;
+		}
+
+		public void Init (AstarPath active, DrawingData.Hasher hasher, DrawingData gizmos, RedrawScope redrawScope) {
+			if (active != null) {
+				debugData = active.debugPathData;
+				debugPathID = active.debugPathID;
+				debugMode = active.debugMode;
+				debugFloor = active.debugFloor;
+				debugRoof = active.debugRoof;
+				nodeStorage = active.nodeStorage;
+#if UNITY_EDITOR
+				if (debugData != null && debugData.threadID < active.nodeStorage.pathfindingThreadData.Length) debugPathNodes = active.nodeStorage.pathfindingThreadData[debugData.threadID].debugPathNodes;
+				else debugPathNodes = default;
+				showSearchTree = active.showSearchTree && debugPathNodes.Length > 0;
+#else
+				showSearchTree = false;
+#endif
+			}
+			this.hasher = hasher;
+			builder = gizmos.GetBuilder(hasher, redrawScope);
+		}
+
+		public void OnEnterPool () {
+			builder.Dispose();
+			debugData = null;
+		}
+
+		public void DrawConnections (GraphNode node) {
+			if (showSearchTree) {
+#if UNITY_EDITOR
+				if (debugPathNodes.Length > 0) {
+					var nodeIndex = node.NodeIndex;
+					for (uint i = 0; i < (uint)node.PathNodeVariants; i++) {
+						var pnode = debugPathNodes[nodeIndex + i];
+						if (pnode.pathID == debugPathID) {
+							if (pnode.parentIndex != 0 && debugPathNodes[pnode.parentIndex].pathID == debugPathID) {
+								var parent = nodeStorage.GetNode(pnode.parentIndex);
+								if (parent != null) {
+									var nodePos = node.DecodeVariantPosition(nodeIndex + i, pnode.fractionAlongEdge);
+									var parentPos = parent.DecodeVariantPosition(pnode.parentIndex, debugPathNodes[pnode.parentIndex].fractionAlongEdge);
+									builder.Line((Vector3)parentPos, (Vector3)nodePos, NodeColor(node));
+								}
+							}
+						}
+					}
+				}
+#endif
+			} else {
+				// Calculate which color to use for drawing the node
+				// based on the settings specified in the editor
+				drawConnectionColor = NodeColor(node);
+				// Get the node position
+				// Cast it here to avoid doing it for every neighbour
+				drawConnectionStart = (Vector3)node.position;
+				node.GetConnections(drawConnection);
+			}
+		}
+
+		void DrawConnection (GraphNode other) {
+			builder.Line(drawConnectionStart, ((Vector3)other.position + drawConnectionStart)*0.5f, drawConnectionColor);
+		}
+
+		/// <summary>
+		/// Color to use for gizmos.
+		/// Returns a color to be used for the specified node with the current debug settings (editor only).
+		///
+		/// Version: Since 3.6.1 this method will not handle null nodes
+		/// </summary>
+		public Color NodeColor (GraphNode node) {
+#if UNITY_EDITOR
+			if (showSearchTree && !InSearchTree(node, debugPathNodes, debugPathID)) return Color.clear;
+#endif
+
+			Color color;
+
+			if (node.Walkable) {
+				switch (debugMode) {
+				case GraphDebugMode.Areas:
+					color = AstarColor.GetAreaColor(node.Area);
+					break;
+				case GraphDebugMode.HierarchicalNode:
+				case GraphDebugMode.NavmeshBorderObstacles:
+					color = AstarColor.GetTagColor((uint)node.HierarchicalNodeIndex);
+					break;
+				case GraphDebugMode.Penalty:
+					color = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, ((float)node.Penalty-debugFloor) / (debugRoof-debugFloor));
+					break;
+				case GraphDebugMode.Tags:
+					color = AstarColor.GetTagColor(node.Tag);
+					break;
+				case GraphDebugMode.SolidColor:
+					color = AstarColor.SolidColor;
+					break;
+				default:
+#if UNITY_EDITOR
+					if (debugPathNodes.Length == 0) {
+						color = AstarColor.SolidColor;
+						break;
+					}
+
+					var pathNode = debugPathNodes[node.NodeIndex];
+					float value;
+					if (debugMode == GraphDebugMode.G) {
+						value = pathNode.g;
+					} else if (debugMode == GraphDebugMode.H) {
+						value = pathNode.h;
+					} else {
+						// mode == F
+						value = pathNode.g + pathNode.h;
+					}
+
+					color = Color.Lerp(AstarColor.ConnectionLowLerp, AstarColor.ConnectionHighLerp, (value-debugFloor) / (debugRoof-debugFloor));
+#else
+					color = AstarColor.SolidColor;
+#endif
+					break;
+				}
+			} else {
+				color = AstarColor.UnwalkableNode;
+			}
+
+			return color;
+		}
+
+#if UNITY_EDITOR
+		/// <summary>
+		/// Returns if the node is in the search tree of the path.
+		/// Only guaranteed to be correct if path is the latest path calculated.
+		/// Use for gizmo drawing only.
+		/// </summary>
+		internal static bool InSearchTree (GraphNode node, UnsafeSpan<GlobalNodeStorage.DebugPathNode> debugPathNodes, ushort pathID) {
+			if (debugPathNodes.Length > 0) {
+				for (uint i = 0; i < node.PathNodeVariants; i++) {
+					if (debugPathNodes[node.NodeIndex + i].pathID == pathID) {
+						return true;
+					}
+				}
+			}
+			return false;
+		}
+#endif
+
+		public void DrawWireTriangle (Vector3 a, Vector3 b, Vector3 c, Color color) {
+			builder.Line(a, b, color);
+			builder.Line(b, c, color);
+			builder.Line(c, a, color);
+		}
+
+		public void DrawTriangles (Vector3[] vertices, Color[] colors, int numTriangles) {
+			var triangles = ArrayPool<int>.Claim(numTriangles*3);
+
+			for (int i = 0; i < numTriangles*3; i++) triangles[i] = i;
+			builder.SolidMesh(vertices, triangles, colors, numTriangles*3, numTriangles*3);
+			ArrayPool<int>.Release(ref triangles);
+		}
+
+		public void DrawWireTriangles (Vector3[] vertices, Color[] colors, int numTriangles) {
+			for (int i = 0; i < numTriangles; i++) {
+				DrawWireTriangle(vertices[i*3+0], vertices[i*3+1], vertices[i*3+2], colors[i*3+0]);
+			}
+		}
+
+		void System.IDisposable.Dispose () {
+			var tmp = this;
+
+			ObjectPool<GraphGizmoHelper>.Release(ref tmp);
+		}
+	}
+}