diff options
Diffstat (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes')
10 files changed, 2323 insertions, 0 deletions
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs new file mode 100644 index 0000000..fe40cb0 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs @@ -0,0 +1,447 @@ +#define PREALLOCATE_NODES +using System.Collections.Generic; +using Pathfinding.Serialization; +using Pathfinding.Util; +using UnityEngine; + +namespace Pathfinding { + /// <summary>Node used for the GridGraph</summary> + public class GridNode : GridNodeBase { + public GridNode() { + } + public GridNode (AstarPath astar) { + astar.InitializeNode(this); + } + +#if !ASTAR_NO_GRID_GRAPH + private static GridGraph[] _gridGraphs = new GridGraph[0]; + public static GridGraph GetGridGraph (uint graphIndex) { return _gridGraphs[(int)graphIndex]; } + + public static void SetGridGraph (int graphIndex, GridGraph graph) { + if (_gridGraphs.Length <= graphIndex) { + var gg = new GridGraph[graphIndex+1]; + for (int i = 0; i < _gridGraphs.Length; i++) gg[i] = _gridGraphs[i]; + _gridGraphs = gg; + } + + _gridGraphs[graphIndex] = graph; + } + + public static void ClearGridGraph (int graphIndex, GridGraph graph) { + if (graphIndex < _gridGraphs.Length && _gridGraphs[graphIndex] == graph) { + _gridGraphs[graphIndex] = null; + } + } + + /// <summary>Internal use only</summary> + internal ushort InternalGridFlags { + get { return gridFlags; } + set { gridFlags = value; } + } + + const int GridFlagsConnectionOffset = 0; + const int GridFlagsConnectionBit0 = 1 << GridFlagsConnectionOffset; + const int GridFlagsConnectionMask = 0xFF << GridFlagsConnectionOffset; + const int GridFlagsAxisAlignedConnectionMask = 0xF << GridFlagsConnectionOffset; + + const int GridFlagsEdgeNodeOffset = 10; + const int GridFlagsEdgeNodeMask = 1 << GridFlagsEdgeNodeOffset; + + public override bool HasConnectionsToAllEightNeighbours { + get { + return (InternalGridFlags & GridFlagsConnectionMask) == GridFlagsConnectionMask; + } + } + + public override bool HasConnectionsToAllAxisAlignedNeighbours { + get { + return (InternalGridFlags & GridFlagsAxisAlignedConnectionMask) == GridFlagsAxisAlignedConnectionMask; + } + } + + /// <summary> + /// True if the node has a connection 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="SetConnectionInternal"/> + /// See: <see cref="GridGraph.neighbourXOffsets"/> + /// See: <see cref="GridGraph.neighbourZOffsets"/> + /// See: <see cref="GridGraph.neighbourOffsets"/> + /// See: <see cref="GridGraph.GetNeighbourIndices"/> + /// </summary> + public override bool HasConnectionInDirection (int dir) { + return (gridFlags >> dir & GridFlagsConnectionBit0) != 0; + } + + /// <summary> + /// True if the node has a connection in the specified direction. + /// Deprecated: Use <see cref="HasConnectionInDirection"/> + /// </summary> + [System.Obsolete("Use HasConnectionInDirection")] + public bool GetConnectionInternal (int dir) { + return HasConnectionInDirection(dir); + } + + /// <summary> + /// Enables or disables a connection in a specified direction on the graph. + /// + /// Note: This only changes the connection from this node to the other node. You may also want to call the same method on the other node with the opposite direction. + /// + /// See: <see cref="HasConnectionInDirection"/> + /// See: <see cref="OppositeConnectionDirection"/> + /// </summary> + public void SetConnection (int dir, bool value) { + SetConnectionInternal(dir, value); + var grid = GetGridGraph(GraphIndex); + grid.nodeDataRef.connections[NodeInGridIndex] = (ulong)GetAllConnectionInternal(); + } + + /// <summary> + /// Enables or disables a connection in a specified direction on the graph. + /// See: <see cref="HasConnectionInDirection"/> + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead, for example <see cref="SetConnection"/>. + /// </summary> + public void SetConnectionInternal (int dir, bool value) { + // Set bit number #dir to 1 or 0 depending on #value + unchecked { gridFlags = (ushort)(gridFlags & ~((ushort)1 << GridFlagsConnectionOffset << dir) | (value ? (ushort)1 : (ushort)0) << GridFlagsConnectionOffset << dir); } + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + /// <summary> + /// Sets the state of all grid connections. + /// + /// See: SetConnectionInternal + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead. + /// </summary> + /// <param name="connections">a bitmask of the connections (bit 0 is the first connection, bit 1 the second connection, etc.).</param> + public void SetAllConnectionInternal (int connections) { + unchecked { gridFlags = (ushort)((gridFlags & ~GridFlagsConnectionMask) | (connections << GridFlagsConnectionOffset)); } + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + /// <summary>Bitpacked int containing all 8 grid connections</summary> + public int GetAllConnectionInternal () { + return (gridFlags & GridFlagsConnectionMask) >> GridFlagsConnectionOffset; + } + + public override bool HasAnyGridConnections => GetAllConnectionInternal() != 0; + + /// <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. + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead. + /// </summary> + public override void ResetConnectionsInternal () { + unchecked { + gridFlags = (ushort)(gridFlags & ~GridFlagsConnectionMask); + } + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + /// <summary> + /// Work in progress for a feature that required info about which nodes were at the border of the graph. + /// Note: This property is not functional at the moment. + /// </summary> + public bool EdgeNode { + get { + return (gridFlags & GridFlagsEdgeNodeMask) != 0; + } + set { + unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsEdgeNodeMask | (value ? GridFlagsEdgeNodeMask : 0)); } + } + } + + public override GridNodeBase GetNeighbourAlongDirection (int direction) { + if (HasConnectionInDirection(direction)) { + GridGraph gg = GetGridGraph(GraphIndex); + return gg.nodes[NodeInGridIndex+gg.neighbourOffsets[direction]]; + } + return null; + } + + public override void ClearConnections (bool alsoReverse) { + if (alsoReverse) { + // Note: This assumes that all connections are bidirectional + // which should hold for all grid graphs unless some custom code has been added + for (int i = 0; i < 8; i++) { + var other = GetNeighbourAlongDirection(i) as GridNode; + if (other != null) { + // Remove reverse connection. See doc for GridGraph.neighbourOffsets to see which indices are used for what. + other.SetConnectionInternal(OppositeConnectionDirection(i), false); + } + } + } + + ResetConnectionsInternal(); + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + base.ClearConnections(alsoReverse); +#endif + } + + public override void GetConnections<T>(GetConnectionsWithData<T> action, ref T data, int connectionFilter) { + if ((connectionFilter & (Connection.IncomingConnection | Connection.OutgoingConnection)) == 0) return; + + GridGraph gg = GetGridGraph(GraphIndex); + + var neighbourOffsets = gg.neighbourOffsets; + var nodes = gg.nodes; + + for (int i = 0; i < 8; i++) { + if ((gridFlags >> i & GridFlagsConnectionBit0) != 0) { + var other = nodes[NodeInGridIndex + neighbourOffsets[i]]; + if (other != null) action(other, ref data); + } + } + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + base.GetConnections(action, ref data, connectionFilter); +#endif + } + + public override bool GetPortal (GraphNode other, out Vector3 left, out Vector3 right) { + if (other.GraphIndex != GraphIndex) { + left = right = Vector3.zero; + return false; + } + + GridGraph gg = GetGridGraph(GraphIndex); + var cellOffset = (other as GridNode).CoordinatesInGrid - CoordinatesInGrid; + var dir = OffsetToConnectionDirection(cellOffset.x, cellOffset.y); + if (dir == -1 || !HasConnectionInDirection(dir)) { + left = right = Vector3.zero; + return false; + } + + UnityEngine.Assertions.Assert.AreEqual(other, gg.nodes[NodeInGridIndex + gg.neighbourOffsets[dir]]); + + if (dir < 4) { + Vector3 middle = ((Vector3)(position + other.position))*0.5f; + Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position-position)); + cross.Normalize(); + cross *= gg.nodeSize*0.5f; + left = middle - cross; + right = middle + cross; + } else { + bool rClear = false; + bool lClear = false; + if (HasConnectionInDirection(dir-4)) { + var n2 = gg.nodes[NodeInGridIndex + gg.neighbourOffsets[dir-4]]; + if (n2.Walkable && n2.HasConnectionInDirection((dir-4+1)%4)) { + rClear = true; + } + } + + if (HasConnectionInDirection((dir-4+1)%4)) { + var n2 = gg.nodes[NodeInGridIndex + gg.neighbourOffsets[(dir-4+1)%4]]; + if (n2.Walkable && n2.HasConnectionInDirection(dir-4)) { + lClear = true; + } + } + + Vector3 middle = ((Vector3)(position + other.position))*0.5f; + Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position-position)); + cross.Normalize(); + cross *= gg.nodeSize*1.4142f; + left = middle - (lClear ? cross : Vector3.zero); + right = middle + (rClear ? cross : Vector3.zero); + } + return true; + } + + /// <summary> + /// Filters diagonal connections based on the non-diagonal ones to prevent corner cutting and similar things. + /// + /// This involves some complicated bitshifting to calculate which diagonal connections + /// should be active based on the non-diagonal ones. + /// For example a path should not be able to pass from A to B if the \<see cref="s"/> represent nodes + /// that we cannot traverse. + /// + /// <code> + /// # B + /// A # + /// </code> + /// + /// Additionally if corner cutting is disabled we will also prevent a connection from A to B in this case: + /// + /// <code> + /// B + /// A # + /// </code> + /// + /// If neighbours = 4 then only the 4 axis aligned connections will be enabled. + /// + /// If neighbours = 6 then only the connections which are valid for hexagonal graphs will be enabled. + /// </summary> + public static int FilterDiagonalConnections (int conns, NumNeighbours neighbours, bool cutCorners) { + switch (neighbours) { + case NumNeighbours.Four: + // The first 4 bits are the axis aligned connections + return conns & 0xF; + // Default case exists only to make the compiler happy, it is never intended to be used. + default: + case NumNeighbours.Eight: + if (cutCorners) { + int axisConns = conns & 0xF; + // If at least one axis aligned connection + // is adjacent to this diagonal, then we can add a connection. + // Bitshifting is a lot faster than calling node.HasConnectionInDirection. + // We need to check if connection i and i+1 are enabled + // but i+1 may overflow 4 and in that case need to be wrapped around + // (so 3+1 = 4 goes to 0). We do that by checking both connection i+1 + // and i+1-4 at the same time. Either i+1 or i+1-4 will be in the range + // from 0 to 4 (exclusive) + int diagConns = (axisConns | (axisConns >> 1 | axisConns << 3)) << 4; + + // Filter out diagonal connections that are invalid + // This will also filter out some junk bits which may be set to true above bit 8 + diagConns &= conns; + return axisConns | diagConns; + } else { + int axisConns = conns & 0xF; + // If exactly 2 axis aligned connections are adjacent to a diagonal connection + // then the diagonal connection is ok to use. + int diagConns = (axisConns & (axisConns >> 1 | axisConns << 3)) << 4; + + // Filter out diagonal connections that are invalid + // This will also filter out some junk bits which may be set above bit 8 + diagConns &= conns; + return axisConns | diagConns; + } + case NumNeighbours.Six: + // Hexagon layout + return conns & GridGraph.HexagonConnectionMask; + } + } + + public override void Open (Path path, uint pathNodeIndex, uint gScore) { + GridGraph gg = GetGridGraph(GraphIndex); + + int[] neighbourOffsets = gg.neighbourOffsets; + uint[] neighbourCosts = gg.neighbourCosts; + var nodes = gg.nodes; + var index = NodeInGridIndex; + + // Bitmask of the 8 connections out of this node + // Each bit represents one connection. + var conns = gridFlags & GridFlagsConnectionMask; + + // Loop over all connections, first the 4 axis aligned ones and then the 4 diagonal ones. + for (int dir = 0; dir < 8; dir++) { + // dir=4 is the first diagonal connection. + // At this point we know exactly which orthogonal (not diagonal) connections are actually traversable. + // So we do some filtering to determine which diagonals should be traversable. + // + // We do this dynamically because each path may use different tags or different + // ITraversalProviders that affect the result. + // + // When the grid graph is scanned this exact method is also run to pre-filter connections + // based on their walkability values. + // Doing pre-filtering is good because it allows users to use `HasConnectionInDirection` + // and it will return accurate values even for diagonals (even though it will of course not + // take into account any additional constraints such as tags or ITraversalProviders). + if (dir == 4 && (path.traversalProvider == null || path.traversalProvider.filterDiagonalGridConnections)) { + conns = FilterDiagonalConnections(conns, gg.neighbours, gg.cutCorners); + } + + // Check if we have a connection in this direction + if (((conns >> dir) & GridFlagsConnectionBit0) != 0) { + var other = nodes[index + neighbourOffsets[dir]]; + if (path.CanTraverse(this, other)) { + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, neighbourCosts[dir], 0, other.position); + } else { + // Mark that connection as not valid + conns &= ~(GridFlagsConnectionBit0 << dir); + } + } + } + + base.Open(path, pathNodeIndex, gScore); + } + + public override void SerializeNode (GraphSerializationContext ctx) { + base.SerializeNode(ctx); + ctx.SerializeInt3(position); + ctx.writer.Write(gridFlags); + } + + public override void DeserializeNode (GraphSerializationContext ctx) { + base.DeserializeNode(ctx); + position = ctx.DeserializeInt3(); + gridFlags = ctx.reader.ReadUInt16(); + } + + public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) { + // In case the node was already added as an internal grid connection, + // we need to remove that connection before we insert it as a custom connection. + // Using a custom connection is necessary because it has a custom cost. + if (node is GridNode gn && gn.GraphIndex == GraphIndex) { + RemoveGridConnection(gn); + } + base.AddPartialConnection(node, cost, isOutgoing, isIncoming); + } + + public override void RemovePartialConnection (GraphNode node) { + base.RemovePartialConnection(node); + // If the node is a grid node on the same graph, it might be added as an internal connection and not a custom one. + if (node is GridNode gn && gn.GraphIndex == GraphIndex) { + RemoveGridConnection(gn); + } + } + + /// <summary> + /// Removes a connection from the internal grid connections. + /// See: SetConnectionInternal + /// </summary> + protected void RemoveGridConnection (GridNode node) { + var nodeIndex = NodeInGridIndex; + var gg = GetGridGraph(GraphIndex); + + for (int i = 0; i < 8; i++) { + if (nodeIndex + gg.neighbourOffsets[i] == node.NodeInGridIndex && GetNeighbourAlongDirection(i) == node) { + SetConnectionInternal(i, false); + break; + } + } + } +#else + public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) { + throw new System.NotImplementedException(); + } + + public override void ClearConnections (bool alsoReverse) { + throw new System.NotImplementedException(); + } + + public override void GetConnections (GraphNodeDelegate del) { + throw new System.NotImplementedException(); + } + + public override void Open (Path path, PathNode pathNode, PathHandler handler) { + throw new System.NotImplementedException(); + } + + public override void AddPartialConnection (GraphNode node) { + throw new System.NotImplementedException(); + } +#endif + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs.meta new file mode 100644 index 0000000..bf0efe9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 5291f1ec332d746138ac025aecb1e12d +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} 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 + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs.meta new file mode 100644 index 0000000..819cad4 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 1b723ddfaa37b46eca23cd8d042ad3e9 +timeCreated: 1459629300 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs new file mode 100644 index 0000000..3a41a08 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs @@ -0,0 +1,403 @@ +#if !ASTAR_NO_GRID_GRAPH +#if !ASTAR_LEVELGRIDNODE_MORE_LAYERS +#define ASTAR_LEVELGRIDNODE_FEW_LAYERS +#endif +using UnityEngine; +using System.Collections.Generic; +using Pathfinding.Serialization; + +namespace Pathfinding { + /// <summary> + /// Describes a single node for the LayerGridGraph. + /// Works almost the same as a grid node, except that it also stores to which layer the connections go to + /// </summary> + public class LevelGridNode : GridNodeBase { + public LevelGridNode() { + } + + public LevelGridNode (AstarPath astar) { + astar.InitializeNode(this); + } + + private static LayerGridGraph[] _gridGraphs = new LayerGridGraph[0]; + public static LayerGridGraph GetGridGraph (uint graphIndex) { return _gridGraphs[(int)graphIndex]; } + + public static void SetGridGraph (int graphIndex, LayerGridGraph graph) { + // LayeredGridGraphs also show up in the grid graph list + // This is required by e.g the XCoordinateInGrid properties + GridNode.SetGridGraph(graphIndex, graph); + if (_gridGraphs.Length <= graphIndex) { + var newGraphs = new LayerGridGraph[graphIndex+1]; + for (int i = 0; i < _gridGraphs.Length; i++) newGraphs[i] = _gridGraphs[i]; + _gridGraphs = newGraphs; + } + + _gridGraphs[graphIndex] = graph; + } + + public static void ClearGridGraph (int graphIndex, LayerGridGraph graph) { + if (graphIndex < _gridGraphs.Length && _gridGraphs[graphIndex] == graph) { + _gridGraphs[graphIndex] = null; + } + } + +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + public uint gridConnections; +#else + public ulong gridConnections; +#endif + + protected static LayerGridGraph[] gridGraphs; + + const int MaxNeighbours = 8; +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + public const int ConnectionMask = 0xF; + public const int ConnectionStride = 4; + public const int AxisAlignedConnectionsMask = 0xFFFF; + public const uint AllConnectionsMask = 0xFFFFFFFF; +#else + public const int ConnectionMask = 0xFF; + public const int ConnectionStride = 8; + public const ulong AxisAlignedConnectionsMask = 0xFFFFFFFF; + public const ulong AllConnectionsMask = 0xFFFFFFFFFFFFFFFF; +#endif + public const int NoConnection = ConnectionMask; + + internal const ulong DiagonalConnectionsMask = ((ulong)NoConnection << 4*ConnectionStride) | ((ulong)NoConnection << 5*ConnectionStride) | ((ulong)NoConnection << 6*ConnectionStride) | ((ulong)NoConnection << 7*ConnectionStride); + + /// <summary> + /// Maximum number of layers the layered grid graph supports. + /// + /// This can be changed in the A* Inspector -> Optimizations tab by enabling or disabling the ASTAR_LEVELGRIDNODE_MORE_LAYERS option. + /// </summary> + public const int MaxLayerCount = ConnectionMask; + + /// <summary> + /// Removes all grid connections from this node. + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead. + /// </summary> + public override void ResetConnectionsInternal () { +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + gridConnections = unchecked ((uint)-1); +#else + gridConnections = unchecked ((ulong)-1); +#endif + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + public override bool HasAnyGridConnections => gridConnections != unchecked ((uint)-1); +#else + public override bool HasAnyGridConnections => gridConnections != unchecked ((ulong)-1); +#endif + + public override bool HasConnectionsToAllEightNeighbours { + get { + for (int i = 0; i < 8; i++) { + if (!HasConnectionInDirection(i)) return false; + } + return true; + } + } + + public override bool HasConnectionsToAllAxisAlignedNeighbours { + get { + return (gridConnections & AxisAlignedConnectionsMask) == AxisAlignedConnectionsMask; + } + } + + /// <summary> + /// Layer coordinate of the node in the grid. + /// If there are multiple nodes in the same (x,z) cell, then they will be stored in different layers. + /// Together with NodeInGridIndex, you can look up the node in the nodes array + /// <code> + /// int index = node.NodeInGridIndex + node.LayerCoordinateInGrid * graph.width * graph.depth; + /// Assert(node == graph.nodes[index]); + /// </code> + /// + /// See: XCoordInGrid + /// See: ZCoordInGrid + /// See: NodeInGridIndex + /// </summary> + public int LayerCoordinateInGrid { get { return nodeInGridIndex >> NodeInGridIndexLayerOffset; } set { nodeInGridIndex = (nodeInGridIndex & NodeInGridIndexMask) | (value << NodeInGridIndexLayerOffset); } } + + public void SetPosition (Int3 position) { + this.position = position; + } + + public override int GetGizmoHashCode () { + return base.GetGizmoHashCode() ^ (int)((805306457UL * gridConnections) ^ (402653189UL * (gridConnections >> 32))); + } + + public override GridNodeBase GetNeighbourAlongDirection (int direction) { + int conn = GetConnectionValue(direction); + + if (conn != NoConnection) { + LayerGridGraph graph = GetGridGraph(GraphIndex); + return graph.nodes[NodeInGridIndex+graph.neighbourOffsets[direction] + graph.lastScannedWidth*graph.lastScannedDepth*conn]; + } + return null; + } + + public override void ClearConnections (bool alsoReverse) { + if (alsoReverse) { + LayerGridGraph graph = GetGridGraph(GraphIndex); + int[] neighbourOffsets = graph.neighbourOffsets; + var nodes = graph.nodes; + + for (int i = 0; i < MaxNeighbours; i++) { + int conn = GetConnectionValue(i); + if (conn != LevelGridNode.NoConnection) { + var other = nodes[NodeInGridIndex+neighbourOffsets[i] + graph.lastScannedWidth*graph.lastScannedDepth*conn] as LevelGridNode; + if (other != null) { + // Remove reverse connection + other.SetConnectionValue((i + 2) % 4, NoConnection); + } + } + } + } + + ResetConnectionsInternal(); + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + base.ClearConnections(alsoReverse); +#endif + } + + public override void GetConnections<T>(GetConnectionsWithData<T> action, ref T data, int connectionFilter) { + if ((connectionFilter & (Connection.IncomingConnection | Connection.OutgoingConnection)) == 0) return; + + LayerGridGraph graph = GetGridGraph(GraphIndex); + + int[] neighbourOffsets = graph.neighbourOffsets; + var nodes = graph.nodes; + int index = NodeInGridIndex; + + for (int i = 0; i < MaxNeighbours; i++) { + int conn = GetConnectionValue(i); + if (conn != LevelGridNode.NoConnection) { + var other = nodes[index+neighbourOffsets[i] + graph.lastScannedWidth*graph.lastScannedDepth*conn]; + if (other != null) action(other, ref data); + } + } + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + base.GetConnections(action, ref data, connectionFilter); +#endif + } + + /// <summary> + /// Is there a grid connection in that direction. + /// + /// Deprecated: Use <see cref="HasConnectionInDirection"/> instead + /// </summary> + [System.Obsolete("Use HasConnectionInDirection instead")] + public bool GetConnection (int i) { + return ((gridConnections >> i*ConnectionStride) & ConnectionMask) != NoConnection; + } + + public override bool HasConnectionInDirection (int direction) { + return ((gridConnections >> direction*ConnectionStride) & ConnectionMask) != NoConnection; + } + + /// <summary> + /// Set which layer a grid connection goes to. + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead. + /// </summary> + /// <param name="dir">Direction for the connection.</param> + /// <param name="value">The layer of the connected node or #NoConnection if there should be no connection in that direction.</param> + public void SetConnectionValue (int dir, int value) { +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + gridConnections = gridConnections & ~(((uint)NoConnection << dir*ConnectionStride)) | ((uint)value << dir*ConnectionStride); +#else + gridConnections = gridConnections & ~(((ulong)NoConnection << dir*ConnectionStride)) | ((ulong)value << dir*ConnectionStride); +#endif + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + public void SetAllConnectionInternal (ulong value) { + gridConnections = (uint)value; + } +#else + public void SetAllConnectionInternal (ulong value) { + gridConnections = value; + } +#endif + + + /// <summary> + /// Which layer a grid connection goes to. + /// Returns: The layer of the connected node or <see cref="NoConnection"/> if there is no connection in that direction. + /// </summary> + /// <param name="dir">Direction for the connection.</param> + public int GetConnectionValue (int dir) { + return (int)((gridConnections >> dir*ConnectionStride) & ConnectionMask); + } + + public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) { + // In case the node was already added as an internal grid connection, + // we need to remove that connection before we insert it as a custom connection. + // Using a custom connection is necessary because it has a custom cost. + if (node is LevelGridNode gn && gn.GraphIndex == GraphIndex) { + RemoveGridConnection(gn); + } + base.AddPartialConnection(node, cost, isOutgoing, isIncoming); + } + + public override void RemovePartialConnection (GraphNode node) { + base.RemovePartialConnection(node); + // If the node is a grid node on the same graph, it might be added as an internal connection and not a custom one. + if (node is LevelGridNode gn && gn.GraphIndex == GraphIndex) { + RemoveGridConnection(gn); + } + } + + /// <summary> + /// Removes a connection from the internal grid connections, not the list of custom connections. + /// See: SetConnectionValue + /// </summary> + protected void RemoveGridConnection (LevelGridNode node) { + var nodeIndex = NodeInGridIndex; + var gg = GetGridGraph(GraphIndex); + + for (int i = 0; i < 8; i++) { + if (nodeIndex + gg.neighbourOffsets[i] == node.NodeInGridIndex && GetNeighbourAlongDirection(i) == node) { + SetConnectionValue(i, NoConnection); + break; + } + } + } + + public override bool GetPortal (GraphNode other, out Vector3 left, out Vector3 right) { + LayerGridGraph graph = GetGridGraph(GraphIndex); + int[] neighbourOffsets = graph.neighbourOffsets; + var nodes = graph.nodes; + int index = NodeInGridIndex; + + for (int i = 0; i < MaxNeighbours; i++) { + int conn = GetConnectionValue(i); + if (conn != LevelGridNode.NoConnection) { + if (other == nodes[index+neighbourOffsets[i] + graph.lastScannedWidth*graph.lastScannedDepth*conn]) { + Vector3 middle = ((Vector3)(position + other.position))*0.5f; + Vector3 cross = Vector3.Cross(graph.collision.up, (Vector3)(other.position-position)); + cross.Normalize(); + cross *= graph.nodeSize*0.5f; + left = middle - cross; + right = middle + cross; + return true; + } + } + } + + left = Vector3.zero; + right = Vector3.zero; + return false; + } + + public override void Open (Path path, uint pathNodeIndex, uint gScore) { + LayerGridGraph graph = GetGridGraph(GraphIndex); + + int[] neighbourOffsets = graph.neighbourOffsets; + uint[] neighbourCosts = graph.neighbourCosts; + var nodes = graph.nodes; + int index = NodeInGridIndex; + + // Bitmask of the 8 connections out of this node. + // Each bit represents one connection. + // We only use this to be able to dynamically handle + // things like cutCorners and other diagonal connection filtering + // based on things like the tags or ITraversalProvider set for just this path. + // It starts off with all connections enabled but then in the following loop + // we will remove connections which are not traversable. + // When we get to the first diagonal connection we run a pass to + // filter out any diagonal connections which shouldn't be enabled. + // See the documentation for FilterDiagonalConnections for more info. + // The regular grid graph does a similar thing. + var conns = 0xFF; + + for (int dir = 0; dir < MaxNeighbours; dir++) { + if (dir == 4 && (path.traversalProvider == null || path.traversalProvider.filterDiagonalGridConnections)) { + conns = GridNode.FilterDiagonalConnections(conns, graph.neighbours, graph.cutCorners); + } + + int conn = GetConnectionValue(dir); + if (conn != LevelGridNode.NoConnection && ((conns >> dir) & 0x1) != 0) { + GraphNode other = nodes[index+neighbourOffsets[dir] + graph.lastScannedWidth*graph.lastScannedDepth*conn]; + + if (!path.CanTraverse(this, other)) { + conns &= ~(1 << dir); + continue; + } + + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, neighbourCosts[dir], 0, other.position); + } else { + conns &= ~(1 << dir); + } + } + + base.Open(path, pathNodeIndex, gScore); + } + + public override void SerializeNode (GraphSerializationContext ctx) { + base.SerializeNode(ctx); + ctx.SerializeInt3(position); + ctx.writer.Write(gridFlags); + // gridConnections are now always serialized as 64 bits for easier compatibility handling +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + // Convert from 32 bits to 64-bits + ulong connectionsLong = 0; + for (int i = 0; i < 8; i++) connectionsLong |= (ulong)GetConnectionValue(i) << (i*8); +#else + ulong connectionsLong = gridConnections; +#endif + ctx.writer.Write(connectionsLong); + } + + public override void DeserializeNode (GraphSerializationContext ctx) { + base.DeserializeNode(ctx); + position = ctx.DeserializeInt3(); + gridFlags = ctx.reader.ReadUInt16(); + if (ctx.meta.version < AstarSerializer.V4_3_12) { + // Note: assumes ASTAR_LEVELGRIDNODE_FEW_LAYERS was false when saving, which was the default + // This info not saved with the graph unfortunately and in 4.3.12 the default changed. + ulong conns; + if (ctx.meta.version < AstarSerializer.V3_9_0) { + // Set the upper 32 bits for compatibility + conns = ctx.reader.ReadUInt32() | (((ulong)NoConnection << 56) | ((ulong)NoConnection << 48) | ((ulong)NoConnection << 40) | ((ulong)NoConnection << 32)); + } else { + conns = ctx.reader.ReadUInt64(); + } + const int stride = 8; + const int mask = (1 << stride) - 1; + gridConnections = 0; + for (int i = 0; i < 8; i++) { + var y = (conns >> (i*stride)) & mask; + // 4.3.12 by default only supports 15 layers. So we may have to disable some connections when loading from earlier versions. + if ((y & ConnectionMask) != y) y = NoConnection; + SetConnectionValue(i, (int)y); + } + } else { + var gridConnectionsLong = ctx.reader.ReadUInt64(); +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + uint c = 0; + if (ctx.meta.version < AstarSerializer.V4_3_83) { + // The default during 4.3.12..4.3.83 was that ASTAR_LEVELGRIDNODE_FEW_LAYERS was enabled, but it was serialized just as 32-bits zero-extended to 64 bits + c = (uint)gridConnectionsLong; + } else { + // Convert from 64 bits to 32-bits + for (int i = 0; i < 8; i++) { + c |= ((uint)(gridConnectionsLong >> (i*8)) & LevelGridNode.ConnectionMask) << (LevelGridNode.ConnectionStride*i); + } + } + gridConnections = c; +#else + gridConnections = gridConnectionsLong; +#endif + } + } + } +} +#endif diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs.meta new file mode 100644 index 0000000..feb761d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e6b8ae51fad7eec4198819399a4e33a2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs new file mode 100644 index 0000000..852fae8 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs @@ -0,0 +1,220 @@ +using UnityEngine; +using Pathfinding.Serialization; + +namespace Pathfinding { + /// <summary> + /// Node used for the PointGraph. + /// This is just a simple point with a list of connections (and associated costs) to other nodes. + /// It does not have any concept of a surface like many other node types. + /// + /// See: PointGraph + /// </summary> + public class PointNode : GraphNode { + /// <summary> + /// All connections from this node. + /// See: <see cref="Connect"/> + /// See: <see cref="Disconnect"/> + /// + /// Note: If you modify this array or the contents of it you must call <see cref="SetConnectivityDirty"/>. + /// + /// Note: If you modify this array or the contents of it you must call <see cref="PointGraph.RegisterConnectionLength"/> with the length of the new connections. + /// </summary> + public Connection[] connections; + + /// <summary> + /// GameObject this node was created from (if any). + /// Warning: When loading a graph from a saved file or from cache, this field will be null. + /// + /// <code> + /// var node = AstarPath.active.GetNearest(transform.position).node; + /// var pointNode = node as PointNode; + /// + /// if (pointNode != null) { + /// Debug.Log("That node was created from the GameObject named " + pointNode.gameObject.name); + /// } else { + /// Debug.Log("That node is not a PointNode"); + /// } + /// </code> + /// </summary> + public GameObject gameObject; + + public void SetPosition (Int3 value) { + position = value; + } + + public PointNode() { } + public PointNode (AstarPath astar) { + astar.InitializeNode(this); + } + + /// <summary> + /// Closest point on the surface of this node to the point p. + /// + /// For a point node this is always the node's <see cref="position"/> sicne it has no surface. + /// </summary> + public override Vector3 ClosestPointOnNode (Vector3 p) { + return (Vector3)this.position; + } + + /// <summary> + /// Checks if point is inside the node when seen from above. + /// + /// Since point nodes have no surface area, this method always returns false. + /// </summary> + public override bool ContainsPoint (Vector3 point) { + return false; + } + + /// <summary> + /// Checks if point is inside the node in graph space. + /// + /// Since point nodes have no surface area, this method always returns false. + /// </summary> + public override bool ContainsPointInGraphSpace (Int3 point) { + return false; + } + + 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 ClearConnections (bool alsoReverse) { + if (alsoReverse && connections != null) { + for (int i = 0; i < connections.Length; i++) { + connections[i].node.RemovePartialConnection(this); + } + } + + connections = null; + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + public override bool ContainsOutgoingConnection (GraphNode node) { + if (connections == null) return false; + for (int i = 0; i < connections.Length; i++) if (connections[i].node == node && connections[i].isOutgoing) return true; + return false; + } + + 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); + + // Make sure the graph knows that there exists a connection with this length + if (this.Graph is PointGraph pg) pg.RegisterConnectionLength((node.position - position).sqrMagnitudeLong); + } + + 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 Open (Path path, uint pathNodeIndex, uint gScore) { + path.OpenCandidateConnectionsToEndNode(position, pathNodeIndex, pathNodeIndex, gScore); + + if (connections == null) return; + + for (int i = 0; i < connections.Length; i++) { + GraphNode other = connections[i].node; + + if (connections[i].isOutgoing && path.CanTraverse(this, other)) { + if (other is PointNode) { + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, connections[i].cost, 0, other.position); + } else { + // When connecting to a non-point node, use a special function to open the connection. + // The typical case for this is that we are at the end of an off-mesh link and we are connecting to a navmesh node. + // In that case, this node's position is in the interior of the navmesh node. We let the navmesh node decide how + // that should be handled. + other.OpenAtPoint(path, pathNodeIndex, position, gScore); + } + } + } + } + + public override void OpenAtPoint (Path path, uint pathNodeIndex, Int3 pos, uint gScore) { + if (path.CanTraverse(this)) { + // TODO: Ideally we should only allow connections to the temporary end node directly from the temporary start node + // iff they lie on the same connection edge. Otherwise we need to pass through the center of this node. + // + // N1---E----N2 + // | / + // | / + // S + // | + // N3 + // + path.OpenCandidateConnectionsToEndNode(pos, pathNodeIndex, pathNodeIndex, gScore); + + var cost = (uint)(pos - this.position).costMagnitude; + path.OpenCandidateConnection(pathNodeIndex, NodeIndex, gScore, cost, 0, position); + } + } + + public override int GetGizmoHashCode () { + var hash = base.GetGizmoHashCode(); + + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + hash ^= 17 * connections[i].GetHashCode(); + } + } + return hash; + } + + public override void SerializeNode (GraphSerializationContext ctx) { + base.SerializeNode(ctx); + ctx.SerializeInt3(position); + } + + public override void DeserializeNode (GraphSerializationContext ctx) { + base.DeserializeNode(ctx); + position = ctx.DeserializeInt3(); + } + + public override void SerializeReferences (GraphSerializationContext ctx) { + ctx.SerializeConnections(connections, true); + } + + public override void DeserializeReferences (GraphSerializationContext ctx) { + connections = ctx.DeserializeConnections(ctx.meta.version >= AstarSerializer.V4_3_85); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs.meta new file mode 100644 index 0000000..00325c7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 986ad6174b59e40068c715a916740ce9 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs new file mode 100644 index 0000000..e7142a6 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs @@ -0,0 +1,673 @@ +#pragma warning disable 0162 +using UnityEngine; +using Pathfinding.Serialization; +using UnityEngine.Assertions; +using Unity.Mathematics; +using Pathfinding.Util; +using Unity.Burst; + +namespace Pathfinding { + /// <summary>Interface for something that holds a triangle based navmesh</summary> + public interface INavmeshHolder : ITransformedGraph, INavmesh { + /// <summary>Position of vertex number i in the world</summary> + Int3 GetVertex(int i); + + /// <summary> + /// Position of vertex number i in coordinates local to the graph. + /// The up direction is always the +Y axis for these coordinates. + /// </summary> + Int3 GetVertexInGraphSpace(int i); + + int GetVertexArrayIndex(int index); + + /// <summary>Transforms coordinates from graph space to world space</summary> + void GetTileCoordinates(int tileIndex, out int x, out int z); + } + + /// <summary>Node represented by a triangle</summary> + [Unity.Burst.BurstCompile] + // Sealing the class provides a nice performance boost (~5-10%) during pathfinding, because the JIT can inline more things and use non-virtual calls. + public sealed class TriangleMeshNode : MeshNode { + public TriangleMeshNode () { + HierarchicalNodeIndex = 0; + NodeIndex = DestroyedNodeIndex; + } + + public TriangleMeshNode (AstarPath astar) { + astar.InitializeNode(this); + } + + /// <summary> + /// Legacy compatibility. + /// Enabling this will make pathfinding use node centers, which leads to less accurate paths (but it's faster). + /// </summary> + public const bool InaccuratePathSearch = false; + internal override int PathNodeVariants => InaccuratePathSearch ? 1 : 3; + + /// <summary>Internal vertex index for the first vertex</summary> + public int v0; + + /// <summary>Internal vertex index for the second vertex</summary> + public int v1; + + /// <summary>Internal vertex index for the third vertex</summary> + public int v2; + + /// <summary>Holds INavmeshHolder references for all graph indices to be able to access them in a performant manner</summary> + static INavmeshHolder[] _navmeshHolders = new INavmeshHolder[0]; + + /// <summary>Used for synchronised access to the <see cref="_navmeshHolders"/> array</summary> + static readonly System.Object lockObject = new System.Object(); + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public static INavmeshHolder GetNavmeshHolder (uint graphIndex) { + return _navmeshHolders[(int)graphIndex]; + } + + /// <summary> + /// Tile index in the recast or navmesh graph that this node is part of. + /// See: <see cref="NavmeshBase.GetTiles"/> + /// </summary> + public int TileIndex => (v0 >> NavmeshBase.TileIndexOffset) & NavmeshBase.TileIndexMask; + + /// <summary> + /// Sets the internal navmesh holder for a given graph index. + /// Warning: Internal method + /// </summary> + public static void SetNavmeshHolder (int graphIndex, INavmeshHolder graph) { + // We need to lock to make sure that + // the resize operation is thread safe + lock (lockObject) { + if (graphIndex >= _navmeshHolders.Length) { + var gg = new INavmeshHolder[graphIndex+1]; + _navmeshHolders.CopyTo(gg, 0); + _navmeshHolders = gg; + } + _navmeshHolders[graphIndex] = graph; + } + } + + public static void ClearNavmeshHolder (int graphIndex, INavmeshHolder graph) { + lock (lockObject) { + if (graphIndex < _navmeshHolders.Length && _navmeshHolders[graphIndex] == graph) { + _navmeshHolders[graphIndex] = null; + } + } + } + + /// <summary>Set the position of this node to the average of its 3 vertices</summary> + public void UpdatePositionFromVertices () { + Int3 a, b, c; + + GetVertices(out a, out b, out c); + position = (a + b + c) * 0.333333f; + } + + /// <summary> + /// Return a number identifying a vertex. + /// This number does not necessarily need to be a index in an array but two different vertices (in the same graph) should + /// not have the same vertex numbers. + /// </summary> + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public int GetVertexIndex (int i) { + return i == 0 ? v0 : (i == 1 ? v1 : v2); + } + + /// <summary> + /// Return a number specifying an index in the source vertex array. + /// The vertex array can for example be contained in a recast tile, or be a navmesh graph, that is graph dependant. + /// This is slower than GetVertexIndex, if you only need to compare vertices, use GetVertexIndex. + /// </summary> + public int GetVertexArrayIndex (int i) { + return GetNavmeshHolder(GraphIndex).GetVertexArrayIndex(i == 0 ? v0 : (i == 1 ? v1 : v2)); + } + + /// <summary>Returns all 3 vertices of this node in world space</summary> + public void GetVertices (out Int3 v0, out Int3 v1, out Int3 v2) { + // Get the object holding the vertex data for this node + // This is usually a graph or a recast graph tile + var holder = GetNavmeshHolder(GraphIndex); + + v0 = holder.GetVertex(this.v0); + v1 = holder.GetVertex(this.v1); + v2 = holder.GetVertex(this.v2); + } + + /// <summary>Returns all 3 vertices of this node in graph space</summary> + public void GetVerticesInGraphSpace (out Int3 v0, out Int3 v1, out Int3 v2) { + // Get the object holding the vertex data for this node + // This is usually a graph or a recast graph tile + var holder = GetNavmeshHolder(GraphIndex); + + v0 = holder.GetVertexInGraphSpace(this.v0); + v1 = holder.GetVertexInGraphSpace(this.v1); + v2 = holder.GetVertexInGraphSpace(this.v2); + } + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public override Int3 GetVertex (int i) { + return GetNavmeshHolder(GraphIndex).GetVertex(GetVertexIndex(i)); + } + + public Int3 GetVertexInGraphSpace (int i) { + return GetNavmeshHolder(GraphIndex).GetVertexInGraphSpace(GetVertexIndex(i)); + } + + public override int GetVertexCount () { + // A triangle has 3 vertices + return 3; + } + + /// <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) { + Int3 a, b, c; + + GetVertices(out a, out b, out c); + var pa = (Vector3)a; + var pb = (Vector3)b; + var pc = (Vector3)c; + var up = Vector3.Cross(pb-pa, pc-pa).normalized; + return point - up * Vector3.Dot(up, point-pa); + } + + public override Vector3 ClosestPointOnNode (Vector3 p) { + Int3 a, b, c; + + GetVertices(out a, out b, out c); + return Pathfinding.Polygon.ClosestPointOnTriangle((float3)(Vector3)a, (float3)(Vector3)b, (float3)(Vector3)c, (float3)p); + } + + /// <summary> + /// Closest point on the node when seen from above. + /// This method is mostly for internal use as the <see cref="Pathfinding.NavmeshBase.Linecast"/> methods use it. + /// + /// - The returned point is the closest one on the node to p when seen from above (relative to the graph). + /// This is important mostly for sloped surfaces. + /// - The returned point is an Int3 point in graph space. + /// - It is guaranteed to be inside the node, so if you call <see cref="ContainsPointInGraphSpace"/> with the return value from this method the result is guaranteed to be true. + /// + /// This method is slower than e.g <see cref="ClosestPointOnNode"/> or <see cref="ClosestPointOnNodeXZ"/>. + /// However they do not have the same guarantees as this method has. + /// </summary> + internal Int3 ClosestPointOnNodeXZInGraphSpace (Vector3 p) { + // Get the vertices that make up the triangle + Int3 a, b, c; + + GetVerticesInGraphSpace(out a, out b, out c); + + // Convert p to graph space + p = GetNavmeshHolder(GraphIndex).transform.InverseTransform(p); + + // Find the closest point on the triangle to p when looking at the triangle from above (relative to the graph) + var closest = Pathfinding.Polygon.ClosestPointOnTriangleXZ((Vector3)a, (Vector3)b, (Vector3)c, p); + + // Make sure the point is actually inside the node + var i3closest = (Int3)closest; + if (ContainsPointInGraphSpace(i3closest)) { + // Common case + return i3closest; + } else { + // Annoying... + // The closest point when converted from floating point coordinates to integer coordinates + // is not actually inside the node. It needs to be inside the node for some methods + // (like for example Linecast) to work properly. + + // Try the 8 integer coordinates around the closest point + // and check if any one of them are completely inside the node. + // This will most likely succeed as it should be very close. + for (int dx = -1; dx <= 1; dx++) { + for (int dz = -1; dz <= 1; dz++) { + if ((dx != 0 || dz != 0)) { + var candidate = new Int3(i3closest.x + dx, i3closest.y, i3closest.z + dz); + if (ContainsPointInGraphSpace(candidate)) return candidate; + } + } + } + + // Happens veery rarely. + // Pick the closest vertex of the triangle. + // The vertex is guaranteed to be inside the triangle. + var da = (a - i3closest).sqrMagnitudeLong; + var db = (b - i3closest).sqrMagnitudeLong; + var dc = (c - i3closest).sqrMagnitudeLong; + return da < db ? (da < dc ? a : c) : (db < dc ? b : c); + } + } + + public override Vector3 ClosestPointOnNodeXZ (Vector3 p) { + // Get all 3 vertices for this node + GetVertices(out Int3 tp1, out Int3 tp2, out Int3 tp3); + return Polygon.ClosestPointOnTriangleXZ((Vector3)tp1, (Vector3)tp2, (Vector3)tp3, p); + } + + /// <summary> + /// Checks if point is inside the node when seen from above. + /// + /// 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 p) { + return ContainsPointInGraphSpace((Int3)GetNavmeshHolder(GraphIndex).transform.InverseTransform(p)); + } + + /// <summary>Checks if point is inside the node when seen from above, as defined by the movement plane</summary> + public bool ContainsPoint (Vector3 p, NativeMovementPlane movementPlane) { + // Get all 3 vertices for this node + GetVertices(out var a, out var b, out var c); + var pa = (int3)a; + var pb = (int3)b; + var pc = (int3)c; + var pp = (int3)(Int3)p; + return Polygon.ContainsPoint(ref pa, ref pb, ref pc, ref pp, ref movementPlane); + } + + /// <summary> + /// Checks if point is inside the node in graph space. + /// + /// In graph space the up direction is always the Y axis so in principle + /// we project the triangle down on the XZ plane and check if the point is inside the 2D triangle there. + /// </summary> + public override bool ContainsPointInGraphSpace (Int3 p) { + // Get all 3 vertices for this node + GetVerticesInGraphSpace(out var a, out var b, out var c); + + if ((long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z) > 0) return false; + + if ((long)(c.x - b.x) * (long)(p.z - b.z) - (long)(p.x - b.x) * (long)(c.z - b.z) > 0) return false; + + if ((long)(a.x - c.x) * (long)(p.z - c.z) - (long)(p.x - c.x) * (long)(a.z - c.z) > 0) return false; + + return true; + // Equivalent code, but the above code is faster + //return Polygon.IsClockwiseMargin (a,b, p) && Polygon.IsClockwiseMargin (b,c, p) && Polygon.IsClockwiseMargin (c,a, p); + + //return Polygon.ContainsPoint(g.GetVertex(v0),g.GetVertex(v1),g.GetVertex(v2),p); + } + + public static readonly Unity.Profiling.ProfilerMarker MarkerDecode = new Unity.Profiling.ProfilerMarker("Decode"); + public static readonly Unity.Profiling.ProfilerMarker MarkerGetVertices = new Unity.Profiling.ProfilerMarker("GetVertex"); + public static readonly Unity.Profiling.ProfilerMarker MarkerClosest = new Unity.Profiling.ProfilerMarker("MarkerClosest"); + + public override Int3 DecodeVariantPosition (uint pathNodeIndex, uint fractionAlongEdge) { + var edge = (int)(pathNodeIndex - NodeIndex); + var p1 = GetVertex(edge); + var p2 = GetVertex((edge + 1) % 3); + InterpolateEdge(ref p1, ref p2, fractionAlongEdge, out var pos); + return pos; + } + + [BurstCompile(FloatMode = FloatMode.Fast)] + static void InterpolateEdge (ref Int3 p1, ref Int3 p2, uint fractionAlongEdge, out Int3 pos) { + var p = (int3)math.lerp((float3)(int3)p1, (float3)(int3)p2, PathNode.UnQuantizeFractionAlongEdge(fractionAlongEdge)); + pos = new Int3(p.x, p.y, p.z); + } + + public override void OpenAtPoint (Path path, uint pathNodeIndex, Int3 point, uint gScore) { + if (InaccuratePathSearch) { + Open(path, pathNodeIndex, gScore); + } else { + OpenAtPoint(path, pathNodeIndex, point, -1, gScore); + } + } + + public override void Open (Path path, uint pathNodeIndex, uint gScore) { + var pathHandler = (path as IPathInternals).PathHandler; + if (InaccuratePathSearch) { + var pn = pathHandler.pathNodes[pathNodeIndex]; + if (pn.flag1) path.OpenCandidateConnectionsToEndNode(position, pathNodeIndex, NodeIndex, gScore); + + if (connections != null) { + // Iterate over all adjacent nodes + for (int i = connections.Length-1; i >= 0; i--) { + var conn = connections[i]; + var other = conn.node; + if (conn.isOutgoing && other.NodeIndex != pn.parentIndex) { + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, conn.cost + path.GetTraversalCost(other), 0, other.position); + } + } + } + return; + } + // One path node variant is created for each side of the triangle + // This particular path node represents just one of the sides of the triangle. + var edge = (int)(pathNodeIndex - NodeIndex); + OpenAtPoint(path, pathNodeIndex, DecodeVariantPosition(pathNodeIndex, pathHandler.pathNodes[pathNodeIndex].fractionAlongEdge), edge, gScore); + } + + void OpenAtPoint (Path path, uint pathNodeIndex, Int3 pos, int edge, uint gScore) { + var pathHandler = (path as IPathInternals).PathHandler; + var pn = pathHandler.pathNodes[pathNodeIndex]; + if (pn.flag1) path.OpenCandidateConnectionsToEndNode(pos, pathNodeIndex, NodeIndex, gScore); + int visitedEdges = 0; + bool cameFromOtherEdgeInThisTriangle = pn.parentIndex >= NodeIndex && pn.parentIndex < NodeIndex + 3; + + if (connections != null) { + // Iterate over all adjacent nodes + for (int i = connections.Length-1; i >= 0; i--) { + var conn = connections[i]; + if (!conn.isOutgoing) continue; + var other = conn.node; + + // Check if we are moving from a side of this triangle, to the corresponding side on an adjacent triangle. + if (conn.isEdgeShared) { + var sharedEdgeOnOtherNode = conn.adjacentShapeEdge; + var adjacentPathNodeIndex = other.NodeIndex + (uint)sharedEdgeOnOtherNode; + + // Skip checking our parent node. This is purely a performance optimization. + if (adjacentPathNodeIndex == pn.parentIndex) continue; + + if (conn.shapeEdge == edge) { + // Make sure we can traverse the neighbour + if (path.CanTraverse(this, other)) { + var tOther = other as TriangleMeshNode; + + // Fast path out if we know we have already searched this node and we cannot improve it + if (!path.ShouldConsiderPathNode(adjacentPathNodeIndex)) { + continue; + } + + if (conn.edgesAreIdentical) { + // The edge on the other node is identical to this edge (but reversed). + // This means that no other node can reach the other node through that edge. + // This is great, because we can then skip adding that node to the heap just + // to immediatelly pop it again. This is a performance optimization. + + var otherEnteringCost = path.GetTraversalCost(other); + ref var otherPathNode = ref pathHandler.pathNodes[adjacentPathNodeIndex]; + otherPathNode.pathID = path.pathID; + otherPathNode.heapIndex = BinaryHeap.NotInHeap; + otherPathNode.parentIndex = pathNodeIndex; + otherPathNode.fractionAlongEdge = PathNode.ReverseFractionAlongEdge(pn.fractionAlongEdge); + // Make sure the path gets information about us having visited this in-between node, + // even if we never add it to the heap + path.OnVisitNode(adjacentPathNodeIndex, uint.MaxValue, gScore + otherEnteringCost); + pathHandler.LogVisitedNode(adjacentPathNodeIndex, uint.MaxValue, gScore + otherEnteringCost); + + tOther.OpenAtPoint(path, adjacentPathNodeIndex, pos, sharedEdgeOnOtherNode, gScore + otherEnteringCost); + } else { + OpenSingleEdge(path, pathNodeIndex, tOther, sharedEdgeOnOtherNode, pos, gScore); + } + } + } else { + // The other node is a node which shares a different edge with this node. + // We will consider this connection at another time. + + // However, we will consider the move to another side of this triangle, + // namely to the side that *is* shared with the other node. + // If a side of this triangle doesn't share an edge with any connection, we will + // not bother searching it (we will not reach this part of the code), because + // we know its a dead end. + + // If we came from another side of this triangle, it is completely redundant to try to move back to + // another edge in this triangle, because we could always have reached it faster from the parent. + // We also make sure we don't attempt to move to the same edge twice, as that's just a waste of time. + if (!cameFromOtherEdgeInThisTriangle && (visitedEdges & (1 << conn.shapeEdge)) == 0) { + visitedEdges |= 1 << conn.shapeEdge; + OpenSingleEdge(path, pathNodeIndex, this, conn.shapeEdge, pos, gScore); + } + } + } else if (!cameFromOtherEdgeInThisTriangle) { + // This is a connection to some other node type, most likely. For example an off-mesh link. + if (path.CanTraverse(this, other) && path.ShouldConsiderPathNode(other.NodeIndex)) { + var cost = (uint)(other.position - pos).costMagnitude; + + if (edge != -1) { + // We are moving from an edge of this triangle + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, cost, 0, other.position); + } else { + // In some situations we may be moving directly from one off-mesh link to another one without + // passing through any concrete nodes in between. In this case we need to create a temporary node + // to allow the correct path to be reconstructed later. The only important part of the temporary + // node is that we save this node as the associated node. + // This is somewhat ugly, and it limits the number of times we can encounter this case during + // a single search (there's a limit to the number of temporary nodes we can have at the same time). + // Fortunately, this case only happens if there is more than 1 off-mesh link connected to a single + // node, which is quite rare in most games. + // In this case, pathNodeIndex will be another node's index, not a path node belonging to this node. + var viaNode = pathHandler.AddTemporaryNode(new TemporaryNode { + associatedNode = NodeIndex, + position = pos, + targetIndex = 0, + type = TemporaryNodeType.Ignore, + }); + ref var viaPathNode = ref pathHandler.pathNodes[viaNode]; + viaPathNode.pathID = path.pathID; + viaPathNode.parentIndex = pathNodeIndex; + path.OpenCandidateConnection(viaNode, other.NodeIndex, gScore, cost, 0, other.position); + } + } + } + } + } + } + + void OpenSingleEdge (Path path, uint pathNodeIndex, TriangleMeshNode other, int sharedEdgeOnOtherNode, Int3 pos, uint gScore) { + var adjacentPathNodeIndex = other.NodeIndex + (uint)sharedEdgeOnOtherNode; + + // Fast path out if we know we have already searched this node and we cannot improve it + if (!path.ShouldConsiderPathNode(adjacentPathNodeIndex)) { + return; + } + + var s1 = other.GetVertex(sharedEdgeOnOtherNode); + var s2 = other.GetVertex((sharedEdgeOnOtherNode + 1) % 3); + + var pathHandler = (path as IPathInternals).PathHandler; + // TODO: Incorrect, counts nodes multiple times + var otherEnteringCost = path.GetTraversalCost(other); + + var candidateG = gScore + otherEnteringCost; + + OpenSingleEdgeBurst( + ref s1, + ref s2, + ref pos, + path.pathID, + pathNodeIndex, + adjacentPathNodeIndex, + other.NodeIndex, + candidateG, + ref pathHandler.pathNodes, + ref pathHandler.heap, + ref path.heuristicObjectiveInternal + ); + } + + [Unity.Burst.BurstCompile] + static void OpenSingleEdgeBurst (ref Int3 s1, ref Int3 s2, ref Int3 pos, ushort pathID, uint pathNodeIndex, uint candidatePathNodeIndex, uint candidateNodeIndex, uint candidateG, ref UnsafeSpan<PathNode> pathNodes, ref BinaryHeap heap, ref HeuristicObjective heuristicObjective) { + CalculateBestEdgePosition(ref s1, ref s2, ref pos, out var closestPointAlongEdge, out var quantizedFractionAlongEdge, out var cost); + candidateG += cost; + + var pars = new Path.OpenCandidateParams { + pathID = pathID, + parentPathNode = pathNodeIndex, + targetPathNode = candidatePathNodeIndex, + targetNodeIndex = candidateNodeIndex, + candidateG = candidateG, + fractionAlongEdge = quantizedFractionAlongEdge, + targetNodePosition = closestPointAlongEdge, + pathNodes = pathNodes, + }; + Path.OpenCandidateConnectionBurst(ref pars, ref heap, ref heuristicObjective); + } + + [Unity.Burst.BurstCompile] + static void CalculateBestEdgePosition (ref Int3 s1, ref Int3 s2, ref Int3 pos, out int3 closestPointAlongEdge, out uint quantizedFractionAlongEdge, out uint cost) { + // Find the closest point on the other edge. From here on, we will let the position of that path node be this closest point. + // This is much better than using the edge midpoint, and also better than any interpolation between closestFractionAlongEdge + // and the midpoint (0.5). + // In my tests, using the edge midpoint leads to path costs that are rougly 1.3-1.6 times greater than the real distance, + // but using the closest point leads to path costs that are only 1.1-1.2 times greater than the real distance. + // Using triangle centers is the worst option, it leads to path costs that are roughly 1.6-2.0 times greater than the real distance. + // Triangle centers were always used before version 4.3.67. + var v1 = (float3)(int3)s1; + var v2 = (float3)(int3)s2; + var posi = (int3)pos; + var closestFractionAlongEdge = math.clamp(VectorMath.ClosestPointOnLineFactor(v1, v2, (float3)posi), 0, 1); + quantizedFractionAlongEdge = PathNode.QuantizeFractionAlongEdge(closestFractionAlongEdge); + closestFractionAlongEdge = PathNode.UnQuantizeFractionAlongEdge(quantizedFractionAlongEdge); + var closestPointAlongEdgeV = math.lerp(v1, v2, closestFractionAlongEdge); + closestPointAlongEdge = (int3)closestPointAlongEdgeV; + + var diff = posi - closestPointAlongEdge; + cost = (uint)new Int3(diff.x, diff.y, diff.z).costMagnitude; + } + + /// <summary> + /// Returns the edge which is shared with other. + /// + /// If there is no shared edge between the two nodes, then -1 is returned. + /// + /// The vertices in the edge can be retrieved using + /// <code> + /// var edge = node.SharedEdge(other); + /// var a = node.GetVertex(edge); + /// var b = node.GetVertex((edge+1) % node.GetVertexCount()); + /// </code> + /// + /// See: <see cref="GetPortal"/> which also handles edges that are shared over tile borders and some types of node links + /// </summary> + public int SharedEdge (GraphNode other) { + var edge = -1; + + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + if (connections[i].node == other && connections[i].isEdgeShared) edge = connections[i].shapeEdge; + } + } + return edge; + } + + public override bool GetPortal (GraphNode toNode, out Vector3 left, out Vector3 right) { + return GetPortal(toNode, out left, out right, out _, out _); + } + + public bool GetPortalInGraphSpace (TriangleMeshNode toNode, out Int3 a, out Int3 b, out int aIndex, out int bIndex) { + aIndex = -1; + bIndex = -1; + a = Int3.zero; + b = Int3.zero; + + // If the nodes are in different graphs, this function has no idea on how to find a shared edge. + if (toNode.GraphIndex != GraphIndex) return false; + + int edge = -1; + int otherEdge = -1; + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + if (connections[i].node == toNode && connections[i].isEdgeShared) { + edge = connections[i].shapeEdge; + otherEdge = connections[i].adjacentShapeEdge; + } + } + } + + // -1: No connection was found between the nodes + if (edge == -1) return false; + + aIndex = edge; + bIndex = (edge + 1) % 3; + + // Get the vertices of the shared edge for the first node + var graph = GetNavmeshHolder(GraphIndex); + a = graph.GetVertexInGraphSpace(GetVertexIndex(aIndex)); + b = graph.GetVertexInGraphSpace(GetVertexIndex(bIndex)); + + // Get tiles the nodes are contained in + int tileIndex1 = TileIndex; + int tileIndex2 = toNode.TileIndex; + + if (tileIndex1 != tileIndex2) { + // When the nodes are in different tiles, the edges may not be completely identical + // so another technique is needed. + + // When the nodes are in different tiles, they might not share exactly the same edge + // so we clamp the portal to the segment of the edges which they both have.. + + // Get the vertices of the shared edge for the second node + Int3 v2a = toNode.GetVertexInGraphSpace(otherEdge); + Int3 v2b = toNode.GetVertexInGraphSpace((otherEdge+1) % 3); + graph.GetTileCoordinates(tileIndex1, out var tileX1, out var tileZ1); + graph.GetTileCoordinates(tileIndex2, out var tileX2, out var tileZ2); + var axis = tileX1 == tileX2 ? 0 : 2; + Assert.IsTrue(axis == 0 ? tileX1 == tileX2 : tileZ1 == tileZ2); + // This tile-edge aligned coordinate of the vertices should ideally be identical. + // But somewhere in the pipeline some errors may crop up, and thus they may be off by one. + // TODO: Fix this. + Assert.IsTrue(Mathf.Abs(a[2 - axis] - b[2 - axis]) <= 1); + var mn = Mathf.Min(v2a[axis], v2b[axis]); + var mx = Mathf.Max(v2a[axis], v2b[axis]); + + a[axis] = Mathf.Clamp(a[axis], mn, mx); + b[axis] = Mathf.Clamp(b[axis], mn, mx); + } + + return true; + } + + public bool GetPortal (GraphNode toNode, out Vector3 left, out Vector3 right, out int aIndex, out int bIndex) { + if (toNode is TriangleMeshNode toTriNode && GetPortalInGraphSpace(toTriNode, out var a, out var b, out aIndex, out bIndex)) { + var graph = GetNavmeshHolder(GraphIndex); + // All triangles should be laid out in clockwise order so b is the rightmost vertex (seen from this node) + left = graph.transform.Transform((Vector3)a); + right = graph.transform.Transform((Vector3)b); + return true; + } else { + aIndex = -1; + bIndex = -1; + left = Vector3.zero; + right = Vector3.zero; + return false; + } + } + + /// <summary>TODO: This is the area in XZ space, use full 3D space for higher correctness maybe?</summary> + public override float SurfaceArea () { + var holder = GetNavmeshHolder(GraphIndex); + + return System.Math.Abs(VectorMath.SignedTriangleAreaTimes2XZ(holder.GetVertex(v0), holder.GetVertex(v1), holder.GetVertex(v2))) * 0.5f; + } + + public override Vector3 RandomPointOnSurface () { + // Find a random point inside the triangle + // This generates uniformly distributed trilinear coordinates + // See http://mathworld.wolfram.com/TrianglePointPicking.html + float2 r; + + do { + r = AstarMath.ThreadSafeRandomFloat2(); + } while (r.x+r.y > 1); + + // Pick the point corresponding to the trilinear coordinate + GetVertices(out var v0, out var v1, out var v2); + return ((Vector3)(v1-v0))*r.x + ((Vector3)(v2-v0))*r.y + (Vector3)v0; + } + + public override void SerializeNode (GraphSerializationContext ctx) { + base.SerializeNode(ctx); + ctx.writer.Write(v0); + ctx.writer.Write(v1); + ctx.writer.Write(v2); + } + + public override void DeserializeNode (GraphSerializationContext ctx) { + base.DeserializeNode(ctx); + v0 = ctx.reader.ReadInt32(); + v1 = ctx.reader.ReadInt32(); + v2 = ctx.reader.ReadInt32(); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs.meta new file mode 100644 index 0000000..c7a35d6 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 54908f58720324c048a5b475a27077fa +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: |