+ astar project

1 files changed, 534 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs
new file mode 100644
index 0000000..53673c1
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs
@@ -0,0 +1,534 @@
+#define PREALLOCATE_NODES
+using UnityEngine;
+using Pathfinding.Serialization;
+
+namespace Pathfinding {
+	/// <summary>Base class for GridNode and LevelGridNode</summary>
+	public abstract class GridNodeBase : GraphNode {
+		const int GridFlagsWalkableErosionOffset = 8;
+		const int GridFlagsWalkableErosionMask = 1 << GridFlagsWalkableErosionOffset;
+
+		const int GridFlagsWalkableTmpOffset = 9;
+		const int GridFlagsWalkableTmpMask = 1 << GridFlagsWalkableTmpOffset;
+
+		public const int NodeInGridIndexLayerOffset = 24;
+		protected const int NodeInGridIndexMask = 0xFFFFFF;
+
+		/// <summary>
+		/// Bitfield containing the x and z coordinates of the node as well as the layer (for layered grid graphs).
+		/// See: NodeInGridIndex
+		/// </summary>
+		protected int nodeInGridIndex;
+		protected ushort gridFlags;
+
+#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
+		/// <summary>
+		/// Custon non-grid connections from this node.
+		/// See: <see cref="Connect"/>
+		/// See: <see cref="Disconnect"/>
+		///
+		/// This field is removed if the ASTAR_GRID_NO_CUSTOM_CONNECTIONS compiler directive is used.
+		/// Removing it can save a tiny bit of memory. You can enable the define in the Optimizations tab in the A* inspector.
+		/// See: compiler-directives (view in online documentation for working links)
+		///
+		/// Note: If you modify this array or the contents of it you must call <see cref="SetConnectivityDirty"/>.
+		/// </summary>
+		public Connection[] connections;
+#endif
+
+		/// <summary>
+		/// The index of the node in the grid.
+		/// This is x + z*graph.width
+		/// So you can get the X and Z indices using
+		/// <code>
+		/// int index = node.NodeInGridIndex;
+		/// int x = index % graph.width;
+		/// int z = index / graph.width;
+		/// // where graph is GridNode.GetGridGraph (node.graphIndex), i.e the graph the nodes are contained in.
+		/// </code>
+		///
+		/// See: <see cref="CoordinatesInGrid"/>
+		/// </summary>
+		public int NodeInGridIndex { get { return nodeInGridIndex & NodeInGridIndexMask; } set { nodeInGridIndex = (nodeInGridIndex & ~NodeInGridIndexMask) | value; } }
+
+		/// <summary>
+		/// X coordinate of the node in the grid.
+		/// The node in the bottom left corner has (x,z) = (0,0) and the one in the opposite
+		/// corner has (x,z) = (width-1, depth-1)
+		///
+		/// See: <see cref="ZCoordinateInGrid"/>
+		/// See: <see cref="NodeInGridIndex"/>
+		/// </summary>
+		public int XCoordinateInGrid => NodeInGridIndex % GridNode.GetGridGraph(GraphIndex).width;
+
+		/// <summary>
+		/// Z coordinate of the node in the grid.
+		/// The node in the bottom left corner has (x,z) = (0,0) and the one in the opposite
+		/// corner has (x,z) = (width-1, depth-1)
+		///
+		/// See: <see cref="XCoordinateInGrid"/>
+		/// See: <see cref="NodeInGridIndex"/>
+		/// </summary>
+		public int ZCoordinateInGrid => NodeInGridIndex / GridNode.GetGridGraph(GraphIndex).width;
+
+		/// <summary>
+		/// The X and Z coordinates of the node in the grid.
+		///
+		/// The node in the bottom left corner has (x,z) = (0,0) and the one in the opposite
+		/// corner has (x,z) = (width-1, depth-1)
+		///
+		/// See: <see cref="XCoordinateInGrid"/>
+		/// See: <see cref="ZCoordinateInGrid"/>
+		/// See: <see cref="NodeInGridIndex"/>
+		/// </summary>
+		public Int2 CoordinatesInGrid {
+			[System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
+			get {
+				var width = GridNode.GetGridGraph(GraphIndex).width;
+				var index = NodeInGridIndex;
+				var z = index / width;
+				var x = index - z * width;
+				return new Int2(x, z);
+			}
+		}
+
+		/// <summary>
+		/// Stores walkability before erosion is applied.
+		/// Used internally when updating the graph.
+		/// </summary>
+		public bool WalkableErosion {
+			get {
+				return (gridFlags & GridFlagsWalkableErosionMask) != 0;
+			}
+			set {
+				unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsWalkableErosionMask | (value ? (ushort)GridFlagsWalkableErosionMask : (ushort)0)); }
+			}
+		}
+
+		/// <summary>Temporary variable used internally when updating the graph.</summary>
+		public bool TmpWalkable {
+			get {
+				return (gridFlags & GridFlagsWalkableTmpMask) != 0;
+			}
+			set {
+				unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsWalkableTmpMask | (value ? (ushort)GridFlagsWalkableTmpMask : (ushort)0)); }
+			}
+		}
+
+		/// <summary>
+		/// True if the node has grid connections to all its 8 neighbours.
+		/// Note: This will always return false if GridGraph.neighbours is set to anything other than Eight.
+		/// See: GetNeighbourAlongDirection
+		/// See: <see cref="HasConnectionsToAllAxisAlignedNeighbours"/>
+		/// </summary>
+		public abstract bool HasConnectionsToAllEightNeighbours { get; }
+
+		/// <summary>
+		/// True if the node has grid connections to all its 4 axis-aligned neighbours.
+		/// See: GetNeighbourAlongDirection
+		/// See: <see cref="HasConnectionsToAllEightNeighbours"/>
+		/// </summary>
+		public abstract bool HasConnectionsToAllAxisAlignedNeighbours { get; }
+
+		/// <summary>
+		/// The connection opposite the given one.
+		///
+		/// <code>
+		///         Z
+		///         |
+		///         |
+		///
+		///      6  2  5
+		///       \ | /
+		/// --  3 - X - 1  ----- X
+		///       / | \
+		///      7  0  4
+		///
+		///         |
+		///         |
+		/// </code>
+		///
+		/// For example, dir=1 outputs 3, dir=6 outputs 4 and so on.
+		///
+		/// See: <see cref="HasConnectionInDirection"/>
+		/// </summary>
+		public static int OppositeConnectionDirection (int dir) {
+			return dir < 4 ? ((dir + 2) % 4) : (((dir-2) % 4) + 4);
+		}
+
+		/// <summary>
+		/// Converts from dx + 3*dz to a neighbour direction.
+		///
+		/// Used by <see cref="OffsetToConnectionDirection"/>.
+		///
+		/// Assumes that dx and dz are both in the range [0,2].
+		/// See: <see cref="GridGraph.neighbourOffsets"/>
+		/// </summary>
+		internal static readonly int[] offsetToDirection = { 7, 0, 4, 3, -1, 1, 6, 2, 5 };
+
+		/// <summary>
+		/// Converts from a delta (dx, dz) to a neighbour direction.
+		///
+		/// For example, if dx=1 and dz=0, the return value will be 1, which is the direction to the right of a grid coordinate.
+		///
+		/// If dx=0 and dz=0, the return value will be -1.
+		///
+		/// See: <see cref="GridGraph.neighbourOffsets"/>
+		///
+		/// <code>
+		///         Z
+		///         |
+		///         |
+		///
+		///      6  2  5
+		///       \ | /
+		/// --  3 - X - 1  ----- X
+		///       / | \
+		///      7  0  4
+		///
+		///         |
+		///         |
+		/// </code>
+		///
+		/// See: <see cref="HasConnectionInDirection"/>
+		/// </summary>
+		/// <param name="dx">X coordinate delta. Should be in the range [-1, 1]. Values outside this range will cause -1 to be returned.</param>
+		/// <param name="dz">Z coordinate delta. Should be in the range [-1, 1]. Values outside this range will cause -1 to be returned.</param>
+		public static int OffsetToConnectionDirection (int dx, int dz) {
+			dx++; dz++;
+			if ((uint)dx > 2 || (uint)dz > 2) return -1;
+			return offsetToDirection[3*dz + dx];
+		}
+
+		/// <summary>
+		/// Projects the given point onto the plane of this node's surface.
+		///
+		/// The point will be projected down to a plane that contains the surface of the node.
+		/// If the point is not contained inside the node, it is projected down onto this plane anyway.
+		/// </summary>
+		public Vector3 ProjectOnSurface (Vector3 point) {
+			var gg = GridNode.GetGridGraph(GraphIndex);
+			var nodeCenter = (Vector3)position;
+			var up = gg.transform.WorldUpAtGraphPosition(nodeCenter);
+			return point - up * Vector3.Dot(up, point - nodeCenter);
+		}
+
+		public override Vector3 ClosestPointOnNode (Vector3 p) {
+			var gg = GridNode.GetGridGraph(GraphIndex);
+
+			// Convert to graph space
+			var nodeCenter = (Vector3)position;
+			// Calculate the offset from the node's center to the given point in graph space
+			var offsetInGraphSpace = gg.transform.InverseTransformVector(p - nodeCenter);
+			// Project onto the node's surface
+			offsetInGraphSpace.y = 0;
+			// Clamp to the node's extents
+			offsetInGraphSpace.x = Mathf.Clamp(offsetInGraphSpace.x, -0.5f, 0.5f);
+			offsetInGraphSpace.z = Mathf.Clamp(offsetInGraphSpace.z, -0.5f, 0.5f);
+			// Convert back to world space
+			return nodeCenter + gg.transform.TransformVector(offsetInGraphSpace);
+		}
+
+		/// <summary>
+		/// Checks if point is inside the node when seen from above.
+		///
+		/// The borders of a node are considered to be inside the node.
+		///
+		/// Note that <see cref="ContainsPointInGraphSpace"/> is faster than this method as it avoids
+		/// some coordinate transformations. If you are repeatedly calling this method
+		/// on many different nodes but with the same point then you should consider
+		/// transforming the point first and then calling ContainsPointInGraphSpace.
+		/// <code>
+		/// Int3 p = (Int3)graph.transform.InverseTransform(point);
+		///
+		/// node.ContainsPointInGraphSpace(p);
+		/// </code>
+		/// </summary>
+		public override bool ContainsPoint (Vector3 point) {
+			var gg = Graph as GridGraph;
+			// Convert to graph space
+			return ContainsPointInGraphSpace((Int3)gg.transform.InverseTransform(point));
+		}
+
+		/// <summary>
+		/// Checks if point is inside the node in graph space.
+		///
+		/// The borders of a node are considered to be inside the node.
+		///
+		/// The y coordinate of the point is ignored.
+		/// </summary>
+		public override bool ContainsPointInGraphSpace (Int3 point) {
+			// Calculate graph position of this node
+			var x = XCoordinateInGrid*Int3.Precision;
+			var z = ZCoordinateInGrid*Int3.Precision;
+
+			return point.x >= x && point.x <= x + Int3.Precision && point.z >= z && point.z <= z + Int3.Precision;
+		}
+
+		public override float SurfaceArea () {
+			GridGraph gg = GridNode.GetGridGraph(GraphIndex);
+
+			return gg.nodeSize*gg.nodeSize;
+		}
+
+		public override Vector3 RandomPointOnSurface () {
+			GridGraph gg = GridNode.GetGridGraph(GraphIndex);
+
+			var graphSpacePosition = gg.transform.InverseTransform((Vector3)position);
+
+			var r = AstarMath.ThreadSafeRandomFloat2();
+			return gg.transform.Transform(graphSpacePosition + new Vector3(r.x - 0.5f, 0, r.y - 0.5f));
+		}
+
+		/// <summary>
+		/// Transforms a world space point to a normalized point on this node's surface.
+		/// (0.5,0.5) represents the node's center. (0,0), (1,0), (1,1) and (0,1) each represent the corners of the node.
+		///
+		/// See: <see cref="UnNormalizePoint"/>
+		/// </summary>
+		public Vector2 NormalizePoint (Vector3 worldPoint) {
+			GridGraph gg = GridNode.GetGridGraph(GraphIndex);
+			var graphSpacePosition = gg.transform.InverseTransform(worldPoint);
+
+			return new Vector2(graphSpacePosition.x - this.XCoordinateInGrid, graphSpacePosition.z - this.ZCoordinateInGrid);
+		}
+
+		/// <summary>
+		/// Transforms a normalized point on this node's surface to a world space point.
+		/// (0.5,0.5) represents the node's center. (0,0), (1,0), (1,1) and (0,1) each represent the corners of the node.
+		///
+		/// See: <see cref="NormalizePoint"/>
+		/// </summary>
+		public Vector3 UnNormalizePoint (Vector2 normalizedPointOnSurface) {
+			GridGraph gg = GridNode.GetGridGraph(GraphIndex);
+
+			return (Vector3)this.position + gg.transform.TransformVector(new Vector3(normalizedPointOnSurface.x - 0.5f, 0, normalizedPointOnSurface.y - 0.5f));
+		}
+
+		public override int GetGizmoHashCode () {
+			var hash = base.GetGizmoHashCode();
+
+#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
+			if (connections != null) {
+				for (int i = 0; i < connections.Length; i++) {
+					hash ^= 17 * connections[i].GetHashCode();
+				}
+			}
+#endif
+			hash ^= 109 * gridFlags;
+			return hash;
+		}
+
+		/// <summary>
+		/// Adjacent grid node in the specified direction.
+		/// This will return null if the node does not have a connection to a node
+		/// in that direction.
+		///
+		/// The dir parameter corresponds to directions in the grid as:
+		/// <code>
+		///         Z
+		///         |
+		///         |
+		///
+		///      6  2  5
+		///       \ | /
+		/// --  3 - X - 1  ----- X
+		///       / | \
+		///      7  0  4
+		///
+		///         |
+		///         |
+		/// </code>
+		///
+		/// See: <see cref="GetConnections"/>
+		/// See: <see cref="GetNeighbourAlongDirection"/>
+		///
+		/// Note: This method only takes grid connections into account, not custom connections (i.e. those added using <see cref="Connect"/> or using node links).
+		/// </summary>
+		public abstract GridNodeBase GetNeighbourAlongDirection(int direction);
+
+		/// <summary>
+		/// True if the node has a connection to an adjecent node in the specified direction.
+		///
+		/// The dir parameter corresponds to directions in the grid as:
+		/// <code>
+		///         Z
+		///         |
+		///         |
+		///
+		///      6  2  5
+		///       \ | /
+		/// --  3 - X - 1  ----- X
+		///       / | \
+		///      7  0  4
+		///
+		///         |
+		///         |
+		/// </code>
+		///
+		/// See: <see cref="GetConnections"/>
+		/// See: <see cref="GetNeighbourAlongDirection"/>
+		/// See: <see cref="OffsetToConnectionDirection"/>
+		///
+		/// Note: This method only takes grid connections into account, not custom connections (i.e. those added using <see cref="Connect"/> or using node links).
+		/// </summary>
+		public virtual bool HasConnectionInDirection (int direction) {
+			// TODO: Can be optimized if overriden in each subclass
+			return GetNeighbourAlongDirection(direction) != null;
+		}
+
+		/// <summary>True if this node has any grid connections</summary>
+		public abstract bool HasAnyGridConnections { get; }
+
+		public override bool ContainsOutgoingConnection (GraphNode node) {
+#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
+			if (connections != null) {
+				for (int i = 0; i < connections.Length; i++) {
+					if (connections[i].node == node && connections[i].isOutgoing) {
+						return true;
+					}
+				}
+			}
+#endif
+
+			for (int i = 0; i < 8; i++) {
+				if (node == GetNeighbourAlongDirection(i)) {
+					return true;
+				}
+			}
+
+			return false;
+		}
+
+		/// <summary>
+		/// Disables all grid connections from this node.
+		/// Note: Other nodes might still be able to get to this node.
+		/// Therefore it is recommended to also disable the relevant connections on adjacent nodes.
+		/// </summary>
+		public abstract void ResetConnectionsInternal();
+
+		public override void OpenAtPoint (Path path, uint pathNodeIndex, Int3 pos, uint gScore) {
+			path.OpenCandidateConnectionsToEndNode(pos, pathNodeIndex, pathNodeIndex, gScore);
+			path.OpenCandidateConnection(pathNodeIndex, NodeIndex, gScore, 0, 0, position);
+		}
+
+		public override void Open (Path path, uint pathNodeIndex, uint gScore) {
+			path.OpenCandidateConnectionsToEndNode(position, pathNodeIndex, pathNodeIndex, gScore);
+
+#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS
+			if (connections != null) {
+				for (int i = 0; i < connections.Length; i++) {
+					GraphNode other = connections[i].node;
+					if (!connections[i].isOutgoing || !path.CanTraverse(this, other)) continue;
+
+					path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, connections[i].cost, 0, other.position);
+				}
+			}
+#endif
+		}
+
+#if ASTAR_GRID_NO_CUSTOM_CONNECTIONS
+		public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) {
+			throw new System.NotImplementedException("GridNodes do not have support for adding manual connections with your current settings."+
+				"\nPlease disable ASTAR_GRID_NO_CUSTOM_CONNECTIONS in the Optimizations tab in the A* Inspector");
+		}
+
+		public override void RemovePartialConnection (GraphNode node) {
+			// Nothing to do because ASTAR_GRID_NO_CUSTOM_CONNECTIONS is enabled
+		}
+
+		public void ClearCustomConnections (bool alsoReverse) {
+		}
+#else
+		/// <summary>Same as <see cref="ClearConnections"/>, but does not clear grid connections, only custom ones (e.g added by <see cref="AddConnection"/> or a NodeLink component)</summary>
+		public void ClearCustomConnections (bool alsoReverse) {
+			if (connections != null) {
+				for (int i = 0; i < connections.Length; i++) connections[i].node.RemovePartialConnection(this);
+				connections = null;
+				AstarPath.active.hierarchicalGraph.AddDirtyNode(this);
+			}
+		}
+
+		public override void ClearConnections (bool alsoReverse) {
+			ClearCustomConnections(alsoReverse);
+		}
+
+		public override void GetConnections<T>(GetConnectionsWithData<T> action, ref T data, int connectionFilter) {
+			if (connections == null) return;
+			for (int i = 0; i < connections.Length; i++) if ((connections[i].shapeEdgeInfo & connectionFilter) != 0) action(connections[i].node, ref data);
+		}
+
+		public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) {
+			if (node == null) throw new System.ArgumentNullException();
+
+			if (connections != null) {
+				for (int i = 0; i < connections.Length; i++) {
+					if (connections[i].node == node) {
+						connections[i].cost = cost;
+						connections[i].shapeEdgeInfo = Connection.PackShapeEdgeInfo(isOutgoing, isIncoming);
+						return;
+					}
+				}
+			}
+
+			int connLength = connections != null ? connections.Length : 0;
+
+			var newconns = new Connection[connLength+1];
+			for (int i = 0; i < connLength; i++) {
+				newconns[i] = connections[i];
+			}
+
+			newconns[connLength] = new Connection(node, cost, isOutgoing, isIncoming);
+
+			connections = newconns;
+			AstarPath.active.hierarchicalGraph.AddDirtyNode(this);
+		}
+
+		/// <summary>
+		/// Removes any connection from this node to the specified node.
+		/// If no such connection exists, nothing will be done.
+		///
+		/// Note: This only removes the connection from this node to the other node.
+		/// You may want to call the same function on the other node to remove its eventual connection
+		/// to this node.
+		///
+		/// Version: Before 4.3.48 This method only handled custom connections (those added using link components or the AddConnection method).
+		/// Regular grid connections had to be added or removed using <see cref="Pathfinding.GridNode.SetConnectionInternal"/>. Starting with 4.3.48 this method
+		/// can remove all types of connections.
+		/// </summary>
+		public override void RemovePartialConnection (GraphNode node) {
+			if (connections == null) return;
+
+			for (int i = 0; i < connections.Length; i++) {
+				if (connections[i].node == node) {
+					int connLength = connections.Length;
+
+					var newconns = new Connection[connLength-1];
+					for (int j = 0; j < i; j++) {
+						newconns[j] = connections[j];
+					}
+					for (int j = i+1; j < connLength; j++) {
+						newconns[j-1] = connections[j];
+					}
+
+					connections = newconns;
+					AstarPath.active.hierarchicalGraph.AddDirtyNode(this);
+					return;
+				}
+			}
+		}
+
+		public override void SerializeReferences (GraphSerializationContext ctx) {
+			ctx.SerializeConnections(connections, true);
+		}
+
+		public override void DeserializeReferences (GraphSerializationContext ctx) {
+			// Grid nodes didn't serialize references before 3.8.3
+			if (ctx.meta.version < AstarSerializer.V3_8_3)
+				return;
+
+			connections = ctx.DeserializeConnections(ctx.meta.version >= AstarSerializer.V4_3_85);
+		}
+#endif
+	}
+}