From 8722a9920c1f6119bf6e769cba270e63097f8e25 Mon Sep 17 00:00:00 2001 From: chai <215380520@qq.com> Date: Thu, 23 May 2024 10:08:29 +0800 Subject: + astar project --- .../Graphs/Nodes/GridNode.cs | 447 ++++++++++++++ .../Graphs/Nodes/GridNode.cs.meta | 7 + .../Graphs/Nodes/GridNodeBase.cs | 534 ++++++++++++++++ .../Graphs/Nodes/GridNodeBase.cs.meta | 12 + .../Graphs/Nodes/LevelGridNode.cs | 403 ++++++++++++ .../Graphs/Nodes/LevelGridNode.cs.meta | 11 + .../Graphs/Nodes/PointNode.cs | 220 +++++++ .../Graphs/Nodes/PointNode.cs.meta | 8 + .../Graphs/Nodes/TriangleMeshNode.cs | 673 +++++++++++++++++++++ .../Graphs/Nodes/TriangleMeshNode.cs.meta | 8 + 10 files changed, 2323 insertions(+) create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs.meta (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes') 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 { + /// Node used for the GridGraph + 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; + } + } + + /// Internal use only + 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; + } + } + + /// + /// True if the node has a connection in the specified direction. + /// The dir parameter corresponds to directions in the grid as: + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// See: + /// See: + /// See: + /// See: + /// See: + /// + public override bool HasConnectionInDirection (int dir) { + return (gridFlags >> dir & GridFlagsConnectionBit0) != 0; + } + + /// + /// True if the node has a connection in the specified direction. + /// Deprecated: Use + /// + [System.Obsolete("Use HasConnectionInDirection")] + public bool GetConnectionInternal (int dir) { + return HasConnectionInDirection(dir); + } + + /// + /// 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: + /// + public void SetConnection (int dir, bool value) { + SetConnectionInternal(dir, value); + var grid = GetGridGraph(GraphIndex); + grid.nodeDataRef.connections[NodeInGridIndex] = (ulong)GetAllConnectionInternal(); + } + + /// + /// Enables or disables a connection in a specified direction on the graph. + /// See: + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead, for example . + /// + 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); + } + + /// + /// 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. + /// + /// a bitmask of the connections (bit 0 is the first connection, bit 1 the second connection, etc.). + public void SetAllConnectionInternal (int connections) { + unchecked { gridFlags = (ushort)((gridFlags & ~GridFlagsConnectionMask) | (connections << GridFlagsConnectionOffset)); } + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + /// Bitpacked int containing all 8 grid connections + public int GetAllConnectionInternal () { + return (gridFlags & GridFlagsConnectionMask) >> GridFlagsConnectionOffset; + } + + public override bool HasAnyGridConnections => GetAllConnectionInternal() != 0; + + /// + /// 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. + /// + public override void ResetConnectionsInternal () { + unchecked { + gridFlags = (ushort)(gridFlags & ~GridFlagsConnectionMask); + } + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + /// + /// 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. + /// + 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(GetConnectionsWithData 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; + } + + /// + /// 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 \ represent nodes + /// that we cannot traverse. + /// + /// + /// # B + /// A # + /// + /// + /// Additionally if corner cutting is disabled we will also prevent a connection from A to B in this case: + /// + /// + /// B + /// A # + /// + /// + /// 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. + /// + 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); + } + } + + /// + /// Removes a connection from the internal grid connections. + /// See: SetConnectionInternal + /// + 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 { + /// Base class for GridNode and LevelGridNode + 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; + + /// + /// Bitfield containing the x and z coordinates of the node as well as the layer (for layered grid graphs). + /// See: NodeInGridIndex + /// + protected int nodeInGridIndex; + protected ushort gridFlags; + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + /// + /// Custon non-grid connections from this node. + /// See: + /// See: + /// + /// 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 . + /// + public Connection[] connections; +#endif + + /// + /// The index of the node in the grid. + /// This is x + z*graph.width + /// So you can get the X and Z indices using + /// + /// 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. + /// + /// + /// See: + /// + public int NodeInGridIndex { get { return nodeInGridIndex & NodeInGridIndexMask; } set { nodeInGridIndex = (nodeInGridIndex & ~NodeInGridIndexMask) | value; } } + + /// + /// 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: + /// + public int XCoordinateInGrid => NodeInGridIndex % GridNode.GetGridGraph(GraphIndex).width; + + /// + /// 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: + /// + public int ZCoordinateInGrid => NodeInGridIndex / GridNode.GetGridGraph(GraphIndex).width; + + /// + /// 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: + /// See: + /// + 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); + } + } + + /// + /// Stores walkability before erosion is applied. + /// Used internally when updating the graph. + /// + public bool WalkableErosion { + get { + return (gridFlags & GridFlagsWalkableErosionMask) != 0; + } + set { + unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsWalkableErosionMask | (value ? (ushort)GridFlagsWalkableErosionMask : (ushort)0)); } + } + } + + /// Temporary variable used internally when updating the graph. + public bool TmpWalkable { + get { + return (gridFlags & GridFlagsWalkableTmpMask) != 0; + } + set { + unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsWalkableTmpMask | (value ? (ushort)GridFlagsWalkableTmpMask : (ushort)0)); } + } + } + + /// + /// 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: + /// + public abstract bool HasConnectionsToAllEightNeighbours { get; } + + /// + /// True if the node has grid connections to all its 4 axis-aligned neighbours. + /// See: GetNeighbourAlongDirection + /// See: + /// + public abstract bool HasConnectionsToAllAxisAlignedNeighbours { get; } + + /// + /// The connection opposite the given one. + /// + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// For example, dir=1 outputs 3, dir=6 outputs 4 and so on. + /// + /// See: + /// + public static int OppositeConnectionDirection (int dir) { + return dir < 4 ? ((dir + 2) % 4) : (((dir-2) % 4) + 4); + } + + /// + /// Converts from dx + 3*dz to a neighbour direction. + /// + /// Used by . + /// + /// Assumes that dx and dz are both in the range [0,2]. + /// See: + /// + internal static readonly int[] offsetToDirection = { 7, 0, 4, 3, -1, 1, 6, 2, 5 }; + + /// + /// 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: + /// + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// See: + /// + /// X coordinate delta. Should be in the range [-1, 1]. Values outside this range will cause -1 to be returned. + /// Z coordinate delta. Should be in the range [-1, 1]. Values outside this range will cause -1 to be returned. + public static int OffsetToConnectionDirection (int dx, int dz) { + dx++; dz++; + if ((uint)dx > 2 || (uint)dz > 2) return -1; + return offsetToDirection[3*dz + dx]; + } + + /// + /// 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. + /// + 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); + } + + /// + /// 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 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. + /// + /// Int3 p = (Int3)graph.transform.InverseTransform(point); + /// + /// node.ContainsPointInGraphSpace(p); + /// + /// + public override bool ContainsPoint (Vector3 point) { + var gg = Graph as GridGraph; + // Convert to graph space + return ContainsPointInGraphSpace((Int3)gg.transform.InverseTransform(point)); + } + + /// + /// 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. + /// + 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)); + } + + /// + /// 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: + /// + 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); + } + + /// + /// 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: + /// + 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; + } + + /// + /// 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: + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// See: + /// See: + /// + /// Note: This method only takes grid connections into account, not custom connections (i.e. those added using or using node links). + /// + public abstract GridNodeBase GetNeighbourAlongDirection(int direction); + + /// + /// 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: + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// See: + /// See: + /// See: + /// + /// Note: This method only takes grid connections into account, not custom connections (i.e. those added using or using node links). + /// + public virtual bool HasConnectionInDirection (int direction) { + // TODO: Can be optimized if overriden in each subclass + return GetNeighbourAlongDirection(direction) != null; + } + + /// True if this node has any grid connections + 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; + } + + /// + /// 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. + /// + 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 + /// Same as , but does not clear grid connections, only custom ones (e.g added by or a NodeLink component) + 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(GetConnectionsWithData 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); + } + + /// + /// 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 . Starting with 4.3.48 this method + /// can remove all types of connections. + /// + 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 { + /// + /// 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 + /// + 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); + + /// + /// 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. + /// + public const int MaxLayerCount = ConnectionMask; + + /// + /// 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. + /// + 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; + } + } + + /// + /// 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 + /// + /// int index = node.NodeInGridIndex + node.LayerCoordinateInGrid * graph.width * graph.depth; + /// Assert(node == graph.nodes[index]); + /// + /// + /// See: XCoordInGrid + /// See: ZCoordInGrid + /// See: NodeInGridIndex + /// + 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(GetConnectionsWithData 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 + } + + /// + /// Is there a grid connection in that direction. + /// + /// Deprecated: Use instead + /// + [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; + } + + /// + /// 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. + /// + /// Direction for the connection. + /// The layer of the connected node or #NoConnection if there should be no connection in that direction. + 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 + + + /// + /// Which layer a grid connection goes to. + /// Returns: The layer of the connected node or if there is no connection in that direction. + /// + /// Direction for the connection. + 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); + } + } + + /// + /// Removes a connection from the internal grid connections, not the list of custom connections. + /// See: SetConnectionValue + /// + 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 { + /// + /// 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 + /// + public class PointNode : GraphNode { + /// + /// All connections from this node. + /// See: + /// See: + /// + /// Note: If you modify this array or the contents of it you must call . + /// + /// Note: If you modify this array or the contents of it you must call with the length of the new connections. + /// + public Connection[] connections; + + /// + /// 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. + /// + /// + /// 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"); + /// } + /// + /// + public GameObject gameObject; + + public void SetPosition (Int3 value) { + position = value; + } + + public PointNode() { } + public PointNode (AstarPath astar) { + astar.InitializeNode(this); + } + + /// + /// Closest point on the surface of this node to the point p. + /// + /// For a point node this is always the node's sicne it has no surface. + /// + public override Vector3 ClosestPointOnNode (Vector3 p) { + return (Vector3)this.position; + } + + /// + /// Checks if point is inside the node when seen from above. + /// + /// Since point nodes have no surface area, this method always returns false. + /// + public override bool ContainsPoint (Vector3 point) { + return false; + } + + /// + /// Checks if point is inside the node in graph space. + /// + /// Since point nodes have no surface area, this method always returns false. + /// + public override bool ContainsPointInGraphSpace (Int3 point) { + return false; + } + + public override void GetConnections(GetConnectionsWithData 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 { + /// Interface for something that holds a triangle based navmesh + public interface INavmeshHolder : ITransformedGraph, INavmesh { + /// Position of vertex number i in the world + Int3 GetVertex(int i); + + /// + /// Position of vertex number i in coordinates local to the graph. + /// The up direction is always the +Y axis for these coordinates. + /// + Int3 GetVertexInGraphSpace(int i); + + int GetVertexArrayIndex(int index); + + /// Transforms coordinates from graph space to world space + void GetTileCoordinates(int tileIndex, out int x, out int z); + } + + /// Node represented by a triangle + [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); + } + + /// + /// Legacy compatibility. + /// Enabling this will make pathfinding use node centers, which leads to less accurate paths (but it's faster). + /// + public const bool InaccuratePathSearch = false; + internal override int PathNodeVariants => InaccuratePathSearch ? 1 : 3; + + /// Internal vertex index for the first vertex + public int v0; + + /// Internal vertex index for the second vertex + public int v1; + + /// Internal vertex index for the third vertex + public int v2; + + /// Holds INavmeshHolder references for all graph indices to be able to access them in a performant manner + static INavmeshHolder[] _navmeshHolders = new INavmeshHolder[0]; + + /// Used for synchronised access to the array + 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]; + } + + /// + /// Tile index in the recast or navmesh graph that this node is part of. + /// See: + /// + public int TileIndex => (v0 >> NavmeshBase.TileIndexOffset) & NavmeshBase.TileIndexMask; + + /// + /// Sets the internal navmesh holder for a given graph index. + /// Warning: Internal method + /// + 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; + } + } + } + + /// Set the position of this node to the average of its 3 vertices + public void UpdatePositionFromVertices () { + Int3 a, b, c; + + GetVertices(out a, out b, out c); + position = (a + b + c) * 0.333333f; + } + + /// + /// 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. + /// + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public int GetVertexIndex (int i) { + return i == 0 ? v0 : (i == 1 ? v1 : v2); + } + + /// + /// 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. + /// + public int GetVertexArrayIndex (int i) { + return GetNavmeshHolder(GraphIndex).GetVertexArrayIndex(i == 0 ? v0 : (i == 1 ? v1 : v2)); + } + + /// Returns all 3 vertices of this node in world space + 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); + } + + /// Returns all 3 vertices of this node in graph space + 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; + } + + /// + /// 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. + /// + 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); + } + + /// + /// Closest point on the node when seen from above. + /// This method is mostly for internal use as the 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 with the return value from this method the result is guaranteed to be true. + /// + /// This method is slower than e.g or . + /// However they do not have the same guarantees as this method has. + /// + 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); + } + + /// + /// Checks if point is inside the node when seen from above. + /// + /// Note that 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. + /// + /// + /// Int3 p = (Int3)graph.transform.InverseTransform(point); + /// + /// node.ContainsPointInGraphSpace(p); + /// + /// + public override bool ContainsPoint (Vector3 p) { + return ContainsPointInGraphSpace((Int3)GetNavmeshHolder(GraphIndex).transform.InverseTransform(p)); + } + + /// Checks if point is inside the node when seen from above, as defined by the movement plane + 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); + } + + /// + /// 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. + /// + 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 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; + } + + /// + /// 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 + /// + /// var edge = node.SharedEdge(other); + /// var a = node.GetVertex(edge); + /// var b = node.GetVertex((edge+1) % node.GetVertexCount()); + /// + /// + /// See: which also handles edges that are shared over tile borders and some types of node links + /// + 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; + } + } + + /// TODO: This is the area in XZ space, use full 3D space for higher correctness maybe? + 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: -- cgit v1.1-26-g67d0