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/Navmesh/AABBTree.cs | 381 ++++++ .../Graphs/Navmesh/AABBTree.cs.meta | 7 + .../Graphs/Navmesh/BBTree.cs | 578 +++++++++ .../Graphs/Navmesh/BBTree.cs.meta | 7 + .../Graphs/Navmesh/ColliderMeshBuilder2D.cs | 336 ++++++ .../Graphs/Navmesh/ColliderMeshBuilder2D.cs.meta | 11 + .../Graphs/Navmesh/Jobs.meta | 8 + .../Graphs/Navmesh/Jobs/JobBuildNodes.cs | 92 ++ .../Graphs/Navmesh/Jobs/JobBuildNodes.cs.meta | 11 + .../Navmesh/Jobs/JobBuildTileMeshFromVertices.cs | 105 ++ .../Jobs/JobBuildTileMeshFromVertices.cs.meta | 11 + .../Navmesh/Jobs/JobBuildTileMeshFromVoxels.cs | 288 +++++ .../Jobs/JobBuildTileMeshFromVoxels.cs.meta | 11 + .../Jobs/JobCalculateTriangleConnections.cs | 73 ++ .../Jobs/JobCalculateTriangleConnections.cs.meta | 11 + .../Graphs/Navmesh/Jobs/JobConnectTiles.cs | 159 +++ .../Graphs/Navmesh/Jobs/JobConnectTiles.cs.meta | 11 + .../Graphs/Navmesh/Jobs/JobConvertAreasToTags.cs | 23 + .../Navmesh/Jobs/JobConvertAreasToTags.cs.meta | 11 + .../Graphs/Navmesh/Jobs/JobCreateTiles.cs | 115 ++ .../Graphs/Navmesh/Jobs/JobCreateTiles.cs.meta | 11 + .../Navmesh/Jobs/JobTransformTileCoordinates.cs | 32 + .../Jobs/JobTransformTileCoordinates.cs.meta | 11 + .../Graphs/Navmesh/Jobs/JobWriteNodeConnections.cs | 60 + .../Navmesh/Jobs/JobWriteNodeConnections.cs.meta | 11 + .../Graphs/Navmesh/NavmeshTile.cs | 106 ++ .../Graphs/Navmesh/NavmeshTile.cs.meta | 12 + .../Graphs/Navmesh/RecastBuilder.cs | 49 + .../Graphs/Navmesh/RecastBuilder.cs.meta | 11 + .../Graphs/Navmesh/RecastMeshGatherer.cs | 1134 ++++++++++++++++++ .../Graphs/Navmesh/RecastMeshGatherer.cs.meta | 11 + .../Graphs/Navmesh/TileBuilder.cs | 366 ++++++ .../Graphs/Navmesh/TileBuilder.cs.meta | 11 + .../Graphs/Navmesh/TileHandler.cs | 1258 ++++++++++++++++++++ .../Graphs/Navmesh/TileHandler.cs.meta | 8 + .../Graphs/Navmesh/TileLayout.cs | 96 ++ .../Graphs/Navmesh/TileLayout.cs.meta | 11 + .../Graphs/Navmesh/TileMesh.cs | 41 + .../Graphs/Navmesh/TileMesh.cs.meta | 11 + .../Graphs/Navmesh/TileMeshes.cs | 193 +++ .../Graphs/Navmesh/TileMeshes.cs.meta | 11 + .../Graphs/Navmesh/Voxels.meta | 2 + .../Graphs/Navmesh/Voxels/CompactVoxelField.cs | 132 ++ .../Navmesh/Voxels/CompactVoxelField.cs.meta | 11 + .../Graphs/Navmesh/Voxels/LinkedVoxelField.cs | 295 +++++ .../Graphs/Navmesh/Voxels/LinkedVoxelField.cs.meta | 11 + .../Graphs/Navmesh/Voxels/VoxelContour.cs | 710 +++++++++++ .../Graphs/Navmesh/Voxels/VoxelContour.cs.meta | 11 + .../Graphs/Navmesh/Voxels/VoxelMesh.cs | 542 +++++++++ .../Graphs/Navmesh/Voxels/VoxelMesh.cs.meta | 11 + .../Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs | 205 ++++ .../Navmesh/Voxels/VoxelPolygonClipper.cs.meta | 12 + .../Graphs/Navmesh/Voxels/VoxelRasterization.cs | 484 ++++++++ .../Navmesh/Voxels/VoxelRasterization.cs.meta | 11 + .../Graphs/Navmesh/Voxels/VoxelRegion.cs | 813 +++++++++++++ .../Graphs/Navmesh/Voxels/VoxelRegion.cs.meta | 11 + 56 files changed, 8964 insertions(+) create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/AABBTree.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/AABBTree.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/BBTree.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/BBTree.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/ColliderMeshBuilder2D.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/ColliderMeshBuilder2D.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildNodes.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildNodes.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVertices.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVertices.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVoxels.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVoxels.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCalculateTriangleConnections.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCalculateTriangleConnections.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConnectTiles.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConnectTiles.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConvertAreasToTags.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConvertAreasToTags.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCreateTiles.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCreateTiles.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobTransformTileCoordinates.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobTransformTileCoordinates.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobWriteNodeConnections.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobWriteNodeConnections.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/NavmeshTile.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/NavmeshTile.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastBuilder.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastBuilder.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastMeshGatherer.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastMeshGatherer.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileBuilder.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileBuilder.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileHandler.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileHandler.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileLayout.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileLayout.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMesh.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMesh.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMeshes.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMeshes.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs.meta (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh') diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/AABBTree.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/AABBTree.cs new file mode 100644 index 0000000..5794204 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/AABBTree.cs @@ -0,0 +1,381 @@ +// #define VALIDATE_AABB_TREE +using UnityEngine; +using System.Collections.Generic; +using Pathfinding.Util; +using Unity.Mathematics; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Navmesh { + /// + /// Axis Aligned Bounding Box Tree. + /// + /// Holds a bounding box tree with arbitrary data. + /// + /// The tree self-balances itself regularly when nodes are added. + /// + public class AABBTree { + Node[] nodes = new Node[0]; + int root = NoNode; + readonly Stack freeNodes = new Stack(); + int rebuildCounter = 64; + const int NoNode = -1; + + struct Node { + public Bounds bounds; + public uint flags; + const uint TagInsideBit = 1u << 30; + const uint TagPartiallyInsideBit = 1u << 31; + const uint AllocatedBit = 1u << 29; + const uint ParentMask = ~(TagInsideBit | TagPartiallyInsideBit | AllocatedBit); + public const int InvalidParent = (int)ParentMask; + public bool wholeSubtreeTagged { + get => (flags & TagInsideBit) != 0; + set => flags = (flags & ~TagInsideBit) | (value ? TagInsideBit : 0); + } + public bool subtreePartiallyTagged { + get => (flags & TagPartiallyInsideBit) != 0; + set => flags = (flags & ~TagPartiallyInsideBit) | (value ? TagPartiallyInsideBit : 0); + } + public bool isAllocated { + get => (flags & AllocatedBit) != 0; + set => flags = (flags & ~AllocatedBit) | (value ? AllocatedBit : 0); + } + public bool isLeaf => left == NoNode; + public int parent { + get => (int)(flags & ParentMask); + set => flags = (flags & ~ParentMask) | (uint)value; + } + public int left; + public int right; + public T value; + } + + /// A key to a leaf node in the tree + public readonly struct Key { + internal readonly int value; + public int node => value - 1; + public bool isValid => value != 0; + internal Key(int node) { this.value = node + 1; } + } + + static float ExpansionRequired (Bounds b, Bounds b2) { + var union = b; + union.Encapsulate(b2); + return union.size.x*union.size.y*union.size.z - b.size.x*b.size.y*b.size.z; + } + + /// User data for a node in the tree + public T this[Key key] => nodes[key.node].value; + + /// Bounding box of a given node + public Bounds GetBounds (Key key) { + if (!key.isValid) throw new System.ArgumentException("Key is not valid"); + var node = nodes[key.node]; + if (!node.isAllocated) throw new System.ArgumentException("Key does not point to an allocated node"); + if (!node.isLeaf) throw new System.ArgumentException("Key does not point to a leaf node"); + return node.bounds; + } + + int AllocNode () { + if (!freeNodes.TryPop(out int newNodeId)) { + int prevLength = nodes.Length; + Memory.Realloc(ref nodes, Mathf.Max(8, nodes.Length*2)); + for (int i = nodes.Length - 1; i >= prevLength; i--) FreeNode(i); + newNodeId = freeNodes.Pop(); +#if VALIDATE_AABB_TREE + Assert.IsFalse(nodes[newNodeId].isAllocated); +#endif + } + return newNodeId; + } + + void FreeNode (int node) { + nodes[node].isAllocated = false; + nodes[node].value = default; + freeNodes.Push(node); + } + + /// + /// Rebuilds the whole tree. + /// + /// This can make it more balanced, and thus faster to query. + /// + public void Rebuild () { + var leaves = new UnsafeSpan(Unity.Collections.Allocator.Temp, nodes.Length); + int nLeaves = 0; + for (int i = 0; i < nodes.Length; i++) { + if (!nodes[i].isAllocated) continue; + if (nodes[i].isLeaf) leaves[nLeaves++] = i; + else FreeNode(i); + } + root = Rebuild(leaves.Slice(0, nLeaves), Node.InvalidParent); + rebuildCounter = Mathf.Max(64, nLeaves / 3); + Validate(root); + } + + /// Removes all nodes from the tree + public void Clear () { + for (int i = 0; i < nodes.Length; i++) { + if (nodes[i].isAllocated) FreeNode(i); + } + root = NoNode; + rebuildCounter = 64; + } + + struct AABBComparer : IComparer { + public Node[] nodes; + public int dim; + + public int Compare(int a, int b) => nodes[a].bounds.center[dim].CompareTo(nodes[b].bounds.center[dim]); + } + + static int ArgMax (Vector3 v) { + var m = Mathf.Max(v.x, Mathf.Max(v.y, v.z)); + return m == v.x ? 0: (m == v.y ? 1 : 2); + } + + int Rebuild (UnsafeSpan leaves, int parent) { + if (leaves.Length == 0) return NoNode; + if (leaves.Length == 1) { + nodes[leaves[0]].parent = parent; + return leaves[0]; + } + + var bounds = nodes[leaves[0]].bounds; + for (int i = 1; i < leaves.Length; i++) bounds.Encapsulate(nodes[leaves[i]].bounds); + + leaves.Sort(new AABBComparer { nodes = nodes, dim = ArgMax(bounds.extents) }); + var nodeId = AllocNode(); + nodes[nodeId] = new Node { + bounds = bounds, + left = Rebuild(leaves.Slice(0, leaves.Length/2), nodeId), + right = Rebuild(leaves.Slice(leaves.Length/2), nodeId), + parent = parent, + isAllocated = true, + }; + return nodeId; + } + + /// + /// Moves a node to a new position. + /// + /// This will update the tree structure to account for the new bounding box. + /// This is equivalent to removing the node and adding it again with the new bounds, but it preserves the key value. + /// + /// Key to the node to move + /// New bounds of the node + public void Move (Key key, Bounds bounds) { + var value = nodes[key.node].value; + Remove(key); + var newKey = Add(bounds, value); + // The first node added after a remove will have the same node index as the just removed node + Assert.IsTrue(newKey.node == key.node); + } + + [System.Diagnostics.Conditional("VALIDATE_AABB_TREE")] + void Validate (int node) { + if (node == NoNode) return; + var n = nodes[node]; + Assert.IsTrue(n.isAllocated); + if (node == root) { + Assert.AreEqual(Node.InvalidParent, n.parent); + } else { + Assert.AreNotEqual(Node.InvalidParent, n.parent); + } + if (n.isLeaf) { + Assert.AreEqual(NoNode, n.right); + } else { + Assert.AreNotEqual(NoNode, n.right); + Assert.AreNotEqual(n.left, n.right); + Assert.AreEqual(node, nodes[n.left].parent); + Assert.AreEqual(node, nodes[n.right].parent); + Assert.IsTrue(math.all((float3)n.bounds.min <= (float3)nodes[n.left].bounds.min + 0.0001f)); + Assert.IsTrue(math.all((float3)n.bounds.max >= (float3)nodes[n.left].bounds.max - 0.0001f)); + Assert.IsTrue(math.all((float3)n.bounds.min <= (float3)nodes[n.right].bounds.min + 0.0001f)); + Assert.IsTrue(math.all((float3)n.bounds.max >= (float3)nodes[n.right].bounds.max - 0.0001f)); + Validate(n.left); + Validate(n.right); + } + } + + public Bounds Remove (Key key) { + if (!key.isValid) throw new System.ArgumentException("Key is not valid"); + var node = nodes[key.node]; + if (!node.isAllocated) throw new System.ArgumentException("Key does not point to an allocated node"); + if (!node.isLeaf) throw new System.ArgumentException("Key does not point to a leaf node"); + + if (key.node == root) { + root = NoNode; + FreeNode(key.node); + return node.bounds; + } + + // Remove the parent from the tree and replace it with sibling + var parentToRemoveId = node.parent; + var parentToRemove = nodes[parentToRemoveId]; + var siblingId = parentToRemove.left == key.node ? parentToRemove.right : parentToRemove.left; + FreeNode(parentToRemoveId); + FreeNode(key.node); + nodes[siblingId].parent = parentToRemove.parent; + + if (parentToRemove.parent == Node.InvalidParent) { + root = siblingId; + } else { + if (nodes[parentToRemove.parent].left == parentToRemoveId) { + nodes[parentToRemove.parent].left = siblingId; + } else { + nodes[parentToRemove.parent].right = siblingId; + } + } + + // Rebuild bounding boxes + var tmpNodeId = nodes[siblingId].parent; + while (tmpNodeId != Node.InvalidParent) { + ref var tmpNode = ref nodes[tmpNodeId]; + var bounds = nodes[tmpNode.left].bounds; + bounds.Encapsulate(nodes[tmpNode.right].bounds); + tmpNode.bounds = bounds; + tmpNode.subtreePartiallyTagged = nodes[tmpNode.left].subtreePartiallyTagged | nodes[tmpNode.right].subtreePartiallyTagged; + tmpNodeId = tmpNode.parent; + } + Validate(root); + return node.bounds; + } + + public Key Add (Bounds bounds, T value) { + var newNodeId = AllocNode(); + + nodes[newNodeId] = new Node { + bounds = bounds, + parent = Node.InvalidParent, + left = NoNode, + right = NoNode, + value = value, + isAllocated = true, + }; + + if (root == NoNode) { + root = newNodeId; + Validate(root); + return new Key(newNodeId); + } + + int nodeId = root; + while (true) { + var node = nodes[nodeId]; + + // We can no longer guarantee that the whole subtree of this node is tagged, + // as the new node is not tagged + nodes[nodeId].wholeSubtreeTagged = false; + + if (node.isLeaf) { + var newInnerId = AllocNode(); + + if (node.parent != Node.InvalidParent) { + if (nodes[node.parent].left == nodeId) nodes[node.parent].left = newInnerId; + else nodes[node.parent].right = newInnerId; + } + + bounds.Encapsulate(node.bounds); + nodes[newInnerId] = new Node { + bounds = bounds, + left = nodeId, + right = newNodeId, + parent = node.parent, + isAllocated = true, + }; + nodes[newNodeId].parent = nodes[nodeId].parent = newInnerId; + if (root == nodeId) root = newInnerId; + + if (rebuildCounter-- <= 0) Rebuild(); + Validate(root); + return new Key(newNodeId); + } else { + // Inner node + nodes[nodeId].bounds.Encapsulate(bounds); + float leftCost = ExpansionRequired(nodes[node.left].bounds, bounds); + float rightCost = ExpansionRequired(nodes[node.right].bounds, bounds); + nodeId = leftCost < rightCost ? node.left : node.right; + } + } + } + + /// Queries the tree for all objects that touch the specified bounds. + /// Bounding box to search within + /// The results will be added to the buffer + public void Query(Bounds bounds, List buffer) => QueryNode(root, bounds, buffer); + + void QueryNode (int node, Bounds bounds, List buffer) { + if (node == NoNode || !bounds.Intersects(nodes[node].bounds)) return; + + if (nodes[node].isLeaf) { + buffer.Add(nodes[node].value); + } else { + // Search children + QueryNode(nodes[node].left, bounds, buffer); + QueryNode(nodes[node].right, bounds, buffer); + } + } + + /// Queries the tree for all objects that have been previously tagged using the method. + /// The results will be added to the buffer + /// If true, all tags will be cleared after this call. If false, the tags will remain and can be queried again later. + public void QueryTagged(List buffer, bool clearTags = false) => QueryTaggedNode(root, clearTags, buffer); + + void QueryTaggedNode (int node, bool clearTags, List buffer) { + if (node == NoNode || !nodes[node].subtreePartiallyTagged) return; + + if (clearTags) { + nodes[node].wholeSubtreeTagged = false; + nodes[node].subtreePartiallyTagged = false; + } + + if (nodes[node].isLeaf) { + buffer.Add(nodes[node].value); + } else { + QueryTaggedNode(nodes[node].left, clearTags, buffer); + QueryTaggedNode(nodes[node].right, clearTags, buffer); + } + } + + /// + /// Tags a particular object. + /// + /// Any previously tagged objects stay tagged. + /// You can retrieve the tagged objects using the method. + /// + /// Key to the object to tag + public void Tag (Key key) { + if (!key.isValid) throw new System.ArgumentException("Key is not valid"); + if (key.node < 0 || key.node >= nodes.Length) throw new System.ArgumentException("Key does not point to a valid node"); + ref var node = ref nodes[key.node]; + if (!node.isAllocated) throw new System.ArgumentException("Key does not point to an allocated node"); + if (!node.isLeaf) throw new System.ArgumentException("Key does not point to a leaf node"); + node.wholeSubtreeTagged = true; + int nodeId = key.node; + while (nodeId != Node.InvalidParent) { + nodes[nodeId].subtreePartiallyTagged = true; + nodeId = nodes[nodeId].parent; + } + } + + /// + /// Tags all objects that touch the specified bounds. + /// + /// Any previously tagged objects stay tagged. + /// You can retrieve the tagged objects using the method. + /// + /// Bounding box to search within + public void Tag(Bounds bounds) => TagNode(root, bounds); + + bool TagNode (int node, Bounds bounds) { + if (node == NoNode || nodes[node].wholeSubtreeTagged) return true; // Nothing to do + if (!bounds.Intersects(nodes[node].bounds)) return false; + + // TODO: Could make this less conservative by propagating info from the child nodes + nodes[node].subtreePartiallyTagged = true; + if (nodes[node].isLeaf) return nodes[node].wholeSubtreeTagged = true; + else return nodes[node].wholeSubtreeTagged = TagNode(nodes[node].left, bounds) & TagNode(nodes[node].right, bounds); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/AABBTree.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/AABBTree.cs.meta new file mode 100644 index 0000000..50515ca --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/AABBTree.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 183e10f9cadca424792b5f940ce3fe3d +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/BBTree.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/BBTree.cs new file mode 100644 index 0000000..c06ec78 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/BBTree.cs @@ -0,0 +1,578 @@ +//#define ASTARDEBUG //"BBTree Debug" If enables, some queries to the tree will show debug lines. Turn off multithreading when using this since DrawLine calls cannot be called from a different thread + +using System; +using System.Collections.Generic; +using UnityEngine; +using Unity.Mathematics; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Pathfinding.Drawing; + +namespace Pathfinding.Graphs.Navmesh { + using Pathfinding.Util; + + /// + /// Axis Aligned Bounding Box Tree. + /// Holds a bounding box tree of triangles. + /// + [BurstCompile] + public struct BBTree : IDisposable { + /// Holds all tree nodes + UnsafeList tree; + UnsafeList nodePermutation; + + const int MaximumLeafSize = 4; + + public IntRect Size => tree.Length == 0 ? default : tree[0].rect; + + // We need a stack while searching the tree. + // We use a stack allocated array for this to avoid allocations. + // A tile can at most contain NavmeshBase.VertexIndexMask triangles. + // This works out to about a million. A perfectly balanced tree can fit this in log2(1000000/4) = 18 levels. + // but we add a few more levels just to be safe, in case the tree is not perfectly balanced. + const int MAX_TREE_HEIGHT = 26; + + public void Dispose () { + nodePermutation.Dispose(); + tree.Dispose(); + } + + /// Build a BBTree from a list of triangles. + /// The triangles. Each triplet of 3 indices represents a node. The triangles are assumed to be in clockwise order. + /// The vertices of the triangles. + public BBTree(UnsafeSpan triangles, UnsafeSpan vertices) { + if (triangles.Length % 3 != 0) throw new ArgumentException("triangles must be a multiple of 3 in length"); + Build(ref triangles, ref vertices, out this); + } + + [BurstCompile] + static void Build (ref UnsafeSpan triangles, ref UnsafeSpan vertices, out BBTree bbTree) { + var nodeCount = triangles.Length/3; + // We will use approximately 2N tree nodes + var tree = new UnsafeList((int)(nodeCount * 2.1f), Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + // We will use approximately N node references + var nodes = new UnsafeList((int)(nodeCount * 1.1f), Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + + // This will store the order of the nodes while the tree is being built + // It turns out that it is a lot faster to do this than to actually modify + // the nodes and nodeBounds arrays (presumably since that involves shuffling + // around 20 bytes of memory (sizeof(pointer) + sizeof(IntRect)) per node + // instead of 4 bytes (sizeof(int)). + // It also means we don't have to make a copy of the nodes array since + // we do not modify it + var permutation = new NativeArray(nodeCount, Allocator.Temp); + for (int i = 0; i < nodeCount; i++) { + permutation[i] = i; + } + + // Precalculate the bounds of the nodes in XZ space. + // It turns out that calculating the bounds is a bottleneck and precalculating + // the bounds makes it around 3 times faster to build a tree + var nodeBounds = new NativeArray(nodeCount, Allocator.Temp); + for (int i = 0; i < nodeCount; i++) { + var v0 = ((int3)vertices[triangles[i*3+0]]).xz; + var v1 = ((int3)vertices[triangles[i*3+1]]).xz; + var v2 = ((int3)vertices[triangles[i*3+2]]).xz; + var mn = math.min(v0, math.min(v1, v2)); + var mx = math.max(v0, math.max(v1, v2)); + nodeBounds[i] = new IntRect(mn.x, mn.y, mx.x, mx.y); + } + + if (nodeCount > 0) BuildSubtree(permutation, nodeBounds, ref nodes, ref tree, 0, nodeCount, false, 0); + nodeBounds.Dispose(); + permutation.Dispose(); + + bbTree = new BBTree { + tree = tree, + nodePermutation = nodes, + }; + } + + static int SplitByX (NativeArray nodesBounds, NativeArray permutation, int from, int to, int divider) { + int mx = to; + + for (int i = from; i < mx; i++) { + var cr = nodesBounds[permutation[i]]; + var cx = (cr.xmin + cr.xmax)/2; + if (cx > divider) { + mx--; + // Swap items i and mx + var tmp = permutation[mx]; + permutation[mx] = permutation[i]; + permutation[i] = tmp; + i--; + } + } + return mx; + } + + static int SplitByZ (NativeArray nodesBounds, NativeArray permutation, int from, int to, int divider) { + int mx = to; + + for (int i = from; i < mx; i++) { + var cr = nodesBounds[permutation[i]]; + var cx = (cr.ymin + cr.ymax)/2; + if (cx > divider) { + mx--; + // Swap items i and mx + var tmp = permutation[mx]; + permutation[mx] = permutation[i]; + permutation[i] = tmp; + i--; + } + } + return mx; + } + + static int BuildSubtree (NativeArray permutation, NativeArray nodeBounds, ref UnsafeList nodes, ref UnsafeList tree, int from, int to, bool odd, int depth) { + var rect = NodeBounds(permutation, nodeBounds, from, to); + int boxId = tree.Length; + tree.Add(new BBTreeBox(rect)); + + if (to - from <= MaximumLeafSize) { + if (depth > MAX_TREE_HEIGHT) { + Debug.LogWarning($"Maximum tree height of {MAX_TREE_HEIGHT} exceeded (got depth of {depth}). Querying this tree may fail. Is the tree very unbalanced?"); + } + var box = tree[boxId]; + var nodeOffset = box.nodeOffset = nodes.Length; + tree[boxId] = box; + nodes.Length += MaximumLeafSize; + // Assign all nodes to the array. Note that we also need clear unused slots as the array from the pool may contain any information + for (int i = 0; i < MaximumLeafSize; i++) { + nodes[nodeOffset + i] = i < to - from ? permutation[from + i] : -1; + } + return boxId; + } else { + int splitIndex; + if (odd) { + // X + int divider = (rect.xmin + rect.xmax)/2; + splitIndex = SplitByX(nodeBounds, permutation, from, to, divider); + } else { + // Y/Z + int divider = (rect.ymin + rect.ymax)/2; + splitIndex = SplitByZ(nodeBounds, permutation, from, to, divider); + } + + int margin = (to - from)/8; + bool veryUneven = splitIndex <= from + margin || splitIndex >= to - margin; + if (veryUneven) { + // All nodes were on one side of the divider + // Try to split along the other axis + + if (!odd) { + // X + int divider = (rect.xmin + rect.xmax)/2; + splitIndex = SplitByX(nodeBounds, permutation, from, to, divider); + } else { + // Y/Z + int divider = (rect.ymin + rect.ymax)/2; + splitIndex = SplitByZ(nodeBounds, permutation, from, to, divider); + } + veryUneven = splitIndex <= from + margin || splitIndex >= to - margin; + + if (veryUneven) { + // Almost all nodes were on one side of the divider + // Just pick one half + splitIndex = (from+to)/2; + } + } + + var left = BuildSubtree(permutation, nodeBounds, ref nodes, ref tree, from, splitIndex, !odd, depth+1); + var right = BuildSubtree(permutation, nodeBounds, ref nodes, ref tree, splitIndex, to, !odd, depth+1); + var box = tree[boxId]; + box.left = left; + box.right = right; + tree[boxId] = box; + + return boxId; + } + } + + /// Calculates the bounding box in XZ space of all nodes between from (inclusive) and to (exclusive) + static IntRect NodeBounds (NativeArray permutation, NativeArray nodeBounds, int from, int to) { + var mn = (int2)nodeBounds[permutation[from]].Min; + var mx = (int2)nodeBounds[permutation[from]].Max; + + for (int j = from + 1; j < to; j++) { + var otherRect = nodeBounds[permutation[j]]; + var rmin = new int2(otherRect.xmin, otherRect.ymin); + var rmax = new int2(otherRect.xmax, otherRect.ymax); + mn = math.min(mn, rmin); + mx = math.max(mx, rmax); + } + + return new IntRect(mn.x, mn.y, mx.x, mx.y); + } + + [BurstCompile] + public readonly struct ProjectionParams { + public readonly float2x3 planeProjection; + public readonly float2 projectedUpNormalized; + public readonly float3 projectionAxis; + public readonly float distanceScaleAlongProjectionAxis; + public readonly DistanceMetric distanceMetric; + // bools are for some reason not blittable by the burst compiler, so we have to use a byte + readonly byte alignedWithXZPlaneBacking; + + public bool alignedWithXZPlane => alignedWithXZPlaneBacking != 0; + + /// + /// Calculates the squared distance from a point to a box when projected to 2D. + /// + /// The input rectangle is assumed to be on the XZ plane, and to actually represent an infinitely tall box (along the Y axis). + /// + /// The planeProjection matrix projects points from 3D to 2D. The box will also be projected. + /// The upProjNormalized vector is the normalized direction orthogonal to the 2D projection. + /// It is the direction pointing out of the plane from the projection's point of view. + /// + /// In the special case that the projection just projects 3D coordinates onto the XZ plane, this is + /// equivalent to the distance from a point to a rectangle in 2D. + /// + public float SquaredRectPointDistanceOnPlane (IntRect rect, float3 p) { + return SquaredRectPointDistanceOnPlane(in this, ref rect, ref p); + } + + [BurstCompile(FloatMode = FloatMode.Fast)] + private static float SquaredRectPointDistanceOnPlane (in ProjectionParams projection, ref IntRect rect, ref float3 p) { + if (projection.alignedWithXZPlane) { + var p1 = new float2(rect.xmin, rect.ymin) * Int3.PrecisionFactor; + var p4 = new float2(rect.xmax, rect.ymax) * Int3.PrecisionFactor; + var closest = math.clamp(p.xz, p1, p4); + return math.lengthsq(closest - p.xz); + } else { + var p1 = new float3(rect.xmin, 0, rect.ymin) * Int3.PrecisionFactor - p; + var p4 = new float3(rect.xmax, 0, rect.ymax) * Int3.PrecisionFactor - p; + var p2 = new float3(rect.xmin, 0, rect.ymax) * Int3.PrecisionFactor - p; + var p3 = new float3(rect.xmax, 0, rect.ymin) * Int3.PrecisionFactor - p; + var p1proj = math.mul(projection.planeProjection, p1); + var p2proj = math.mul(projection.planeProjection, p2); + var p3proj = math.mul(projection.planeProjection, p3); + var p4proj = math.mul(projection.planeProjection, p4); + var upNormal = new float2(projection.projectedUpNormalized.y, -projection.projectedUpNormalized.x); + // Calculate the dot product of pNproj and upNormal for all N, this is the distance between p and pN + // along the direction orthogonal to upProjNormalized. + // The box is infinite along the up direction (since it is only a rect). When projected down to 2D + // this results in an infinite line with a given thickness (a beam). + // This is assuming the projection direction is not parallel to the world up direction, in which case we + // would have entered the other branch of this if statement. + // The minumum value and maximum value in dists gives us the signed distance to this beam + // from the point p. + var dists = math.mul(math.transpose(new float2x4(p1proj, p2proj, p3proj, p4proj)), upNormal); + // Calculate the shortest distance to the beam (may be 0 if p is inside the beam). + var dist = math.clamp(0, math.cmin(dists), math.cmax(dists)); + return dist*dist; + } + } + + public ProjectionParams(NNConstraint constraint, GraphTransform graphTransform) { + const float MAX_ERROR_IN_RADIANS = 0.01f; + + // The normal of the plane we are projecting onto (if any). + if (constraint != null && constraint.distanceMetric.projectionAxis != Vector3.zero) { + // (inf,inf,inf) is a special value indicating to use the graph's natural up direction + if (float.IsPositiveInfinity(constraint.distanceMetric.projectionAxis.x)) { + projectionAxis = new float3(0, 1, 0); + } else { + projectionAxis = math.normalizesafe(graphTransform.InverseTransformVector(constraint.distanceMetric.projectionAxis)); + } + + if (projectionAxis.x*projectionAxis.x + projectionAxis.z*projectionAxis.z < MAX_ERROR_IN_RADIANS*MAX_ERROR_IN_RADIANS) { + // We could let the code below handle this case, but since it is a common case we can optimize it a bit + // by using a fast-path here. + projectedUpNormalized = float2.zero; + planeProjection = new float2x3(1, 0, 0, 0, 0, 1); // math.transpose(new float3x2(new float3(1, 0, 0), new float3(0, 0, 1))); + distanceMetric = DistanceMetric.ScaledManhattan; + alignedWithXZPlaneBacking = (byte)1; + distanceScaleAlongProjectionAxis = math.max(constraint.distanceMetric.distanceScaleAlongProjectionDirection, 0); + return; + } + + // Find any two vectors which are perpendicular to the normal (and each other) + var planeAxis1 = math.normalizesafe(math.cross(new float3(1, 0, 1), projectionAxis)); + + if (math.all(planeAxis1 == 0)) planeAxis1 = math.normalizesafe(math.cross(new float3(-1, 0, 1), projectionAxis)); + var planeAxis2 = math.normalizesafe(math.cross(projectionAxis, planeAxis1)); + // Note: The inverse of an orthogonal matrix is its transpose, and the transpose is faster to compute + planeProjection = math.transpose(new float3x2(planeAxis1, planeAxis2)); + // The projection of the (0,1,0) vector onto the plane. + // This is important because the BBTree stores its rectangles in the XZ plane. + // If the projection is close enough to the XZ plane, we snap to that because it allows us to use faster and more precise distance calculations. + projectedUpNormalized = math.lengthsq(planeProjection.c1) <= MAX_ERROR_IN_RADIANS*MAX_ERROR_IN_RADIANS ? float2.zero : math.normalize(planeProjection.c1); + distanceMetric = DistanceMetric.ScaledManhattan; + alignedWithXZPlaneBacking = math.all(projectedUpNormalized == 0) ? (byte)1 : (byte)0; + + // The distance along the projection axis is scaled by a cost factor to make the distance + // along the projection direction more or less important compared to the distance in the plane. + // Usually the projection direction is less important. + // For example, when an agent looks for the closest node, it is typically more interested in finding a point close + // to it which is more or less directly below it, than it is in finding a point which is closer, but requires sideways movement. + // Even if this value is zero we will use the distance along the projection axis to break ties. + // Otherwise, when getting the nearest node in e.g. a tall building, it would not be well defined + // which floor of the building was closest. + distanceScaleAlongProjectionAxis = math.max(constraint.distanceMetric.distanceScaleAlongProjectionDirection, 0); + } else { + projectionAxis = float3.zero; + planeProjection = default; + projectedUpNormalized = default; + distanceMetric = DistanceMetric.Euclidean; + alignedWithXZPlaneBacking = 1; + distanceScaleAlongProjectionAxis = 0; + } + } + } + + public float DistanceSqrLowerBound (float3 p, in ProjectionParams projection) { + if (tree.Length == 0) return float.PositiveInfinity; + return projection.SquaredRectPointDistanceOnPlane(tree[0].rect, p); + } + + /// + /// Queries the tree for the closest node to p constrained by the NNConstraint trying to improve an existing solution. + /// Note that this function will only fill in the constrained node. + /// If you want a node not constrained by any NNConstraint, do an additional search with constraint = NNConstraint.None + /// + /// Point to search around + /// Optionally set to constrain which nodes to return + /// The best squared distance for the previous solution. Will be updated with the best distance + /// after this search. Supply positive infinity to start the search from scratch. + /// This search will start from the previous NNInfo and improve it if possible. Will be updated with the new result. + /// Even if the search fails on this call, the solution will never be worse than previous. + /// The nodes what this BBTree was built from + /// The triangles that this BBTree was built from + /// The vertices that this BBTree was built from + /// Projection parameters derived from the constraint + public void QueryClosest (float3 p, NNConstraint constraint, in ProjectionParams projection, ref float distanceSqr, ref NNInfo previous, GraphNode[] nodes, UnsafeSpan triangles, UnsafeSpan vertices) { + if (tree.Length == 0) return; + + UnsafeSpan stack; + unsafe { + NearbyNodesIterator.BoxWithDist* stackPtr = stackalloc NearbyNodesIterator.BoxWithDist[MAX_TREE_HEIGHT]; + stack = new UnsafeSpan(stackPtr, MAX_TREE_HEIGHT); + } + stack[0] = new NearbyNodesIterator.BoxWithDist { + index = 0, + distSqr = 0.0f, + }; + var it = new NearbyNodesIterator { + stack = stack, + stackSize = 1, + indexInLeaf = 0, + point = p, + projection = projection, + distanceThresholdSqr = distanceSqr, + tieBreakingDistanceThreshold = float.PositiveInfinity, + tree = tree.AsUnsafeSpan(), + nodes = nodePermutation.AsUnsafeSpan(), + triangles = triangles, + vertices = vertices, + }; + + // We use an iterator which searches through the tree and returns nodes closer than it.distanceThresholdSqr. + // The iterator is compiled using burst for high performance, but when a new candidate node is found we need + // to evaluate it in pure C# due to the NNConstraint being a C# class. + // TODO: If constraint==null (or NNConstraint.None) we could run the whole thing in burst to improve perf even more. + var result = previous; + while (it.stackSize > 0 && it.MoveNext()) { + var current = it.current; + if (constraint == null || constraint.Suitable(nodes[current.node])) { + it.distanceThresholdSqr = current.distanceSq; + it.tieBreakingDistanceThreshold = current.tieBreakingDistance; + result = new NNInfo(nodes[current.node], current.closestPointOnNode, current.distanceSq); + } + } + distanceSqr = it.distanceThresholdSqr; + previous = result; + } + + struct CloseNode { + public int node; + public float distanceSq; + public float tieBreakingDistance; + public float3 closestPointOnNode; + } + + public enum DistanceMetric: byte { + Euclidean, + ScaledManhattan, + } + + [BurstCompile] + struct NearbyNodesIterator : IEnumerator { + public UnsafeSpan stack; + public int stackSize; + public UnsafeSpan tree; + public UnsafeSpan nodes; + public UnsafeSpan triangles; + public UnsafeSpan vertices; + public int indexInLeaf; + public float3 point; + public ProjectionParams projection; + public float distanceThresholdSqr; + public float tieBreakingDistanceThreshold; + internal CloseNode current; + + public CloseNode Current => current; + + public struct BoxWithDist { + public int index; + public float distSqr; + } + + public bool MoveNext () { + return MoveNext(ref this); + } + + void IDisposable.Dispose () {} + + void System.Collections.IEnumerator.Reset() => throw new NotSupportedException(); + object System.Collections.IEnumerator.Current => throw new NotSupportedException(); + + // Note: Using FloatMode=Fast here can cause NaNs in rare cases. + // I have not tracked down why, but it is not unreasonable given that FloatMode=Fast assumes that infinities do not happen. + [BurstCompile(FloatMode = FloatMode.Default)] + static bool MoveNext (ref NearbyNodesIterator it) { + var distanceThresholdSqr = it.distanceThresholdSqr; + while (true) { + if (it.stackSize == 0) { + return false; + } + + // Pop the last element from the stack + var boxRef = it.stack[it.stackSize-1]; + + // If we cannot possibly find anything better than the current best solution in here, skip this box. + // Allow the search when we can find an equally close node, because tie breaking + // may cause this search to find a better node. + if (boxRef.distSqr > distanceThresholdSqr) { + it.stackSize--; + // Setting this to zero shouldn't be necessary in theory, as a leaf will always (in theory) be searched completely. + // However, in practice the distance to a node may be a tiny bit lower than the distance to the box containing the node, due to floating point errors. + // and so the leaf's search may be terminated early if a point is found on a node exactly on the border of the box. + // In that case it is important that we reset the iterator to the start of the next leaf. + it.indexInLeaf = 0; + continue; + } + + BBTreeBox box = it.tree[boxRef.index]; + if (box.IsLeaf) { + for (int i = it.indexInLeaf; i < MaximumLeafSize; i++) { + var node = it.nodes[box.nodeOffset + i]; + if (node == -1) break; + var ti1 = (uint)(node*3 + 0); + var ti2 = (uint)(node*3 + 1); + var ti3 = (uint)(node*3 + 2); + if (ti3 >= it.triangles.length) throw new Exception("Invalid node index"); + Unity.Burst.CompilerServices.Hint.Assume(ti1 < it.triangles.length && ti2 < it.triangles.length && ti3 < it.triangles.length); + var vi1 = it.vertices[it.triangles[ti1]]; + var vi2 = it.vertices[it.triangles[ti2]]; + var vi3 = it.vertices[it.triangles[ti3]]; + if (it.projection.distanceMetric == DistanceMetric.Euclidean) { + var v1 = (float3)vi1; + var v2 = (float3)vi2; + var v3 = (float3)vi3; + Polygon.ClosestPointOnTriangleByRef(in v1, in v2, in v3, in it.point, out var closest); + var sqrDist = math.distancesq(closest, it.point); + if (sqrDist < distanceThresholdSqr) { + it.indexInLeaf = i + 1; + it.current = new CloseNode { + node = node, + distanceSq = sqrDist, + tieBreakingDistance = 0, + closestPointOnNode = closest, + }; + return true; + } + } else { + Polygon.ClosestPointOnTriangleProjected(ref vi1, ref vi2, ref vi3, ref it.projection, ref it.point, out var closest, out var sqrDist, out var distAlongProjection); + // Check if this point is better than the previously best point. + // Handling ties here is important, in case the navmesh has multiple overlapping regions (e.g. a multi-story building). + if (sqrDist < distanceThresholdSqr || (sqrDist == distanceThresholdSqr && distAlongProjection < it.tieBreakingDistanceThreshold)) { + it.indexInLeaf = i + 1; + it.current = new CloseNode { + node = node, + distanceSq = sqrDist, + tieBreakingDistance = distAlongProjection, + closestPointOnNode = closest, + }; + return true; + } + } + } + it.indexInLeaf = 0; + it.stackSize--; + } else { + it.stackSize--; + + int first = box.left, second = box.right; + var firstDist = it.projection.SquaredRectPointDistanceOnPlane(it.tree[first].rect, it.point); + var secondDist = it.projection.SquaredRectPointDistanceOnPlane(it.tree[second].rect, it.point); + + if (secondDist < firstDist) { + // Swap + Memory.Swap(ref first, ref second); + Memory.Swap(ref firstDist, ref secondDist); + } + + if (it.stackSize + 2 > it.stack.Length) { + throw new InvalidOperationException("Tree is too deep. Overflowed the internal stack."); + } + + // Push both children on the stack so that we can explore them later (if they are not too far away). + // We push the one with the smallest distance last so that it will be popped first. + if (secondDist <= distanceThresholdSqr) it.stack[it.stackSize++] = new BoxWithDist { + index = second, + distSqr = secondDist, + }; + if (firstDist <= distanceThresholdSqr) it.stack[it.stackSize++] = new BoxWithDist { + index = first, + distSqr = firstDist, + }; + } + } + } + } + + struct BBTreeBox { + public IntRect rect; + + public int nodeOffset; + public int left, right; + + public bool IsLeaf => nodeOffset >= 0; + + public BBTreeBox (IntRect rect) { + nodeOffset = -1; + this.rect = rect; + left = right = -1; + } + } + + public void DrawGizmos (CommandBuilder draw) { + Gizmos.color = new Color(1, 1, 1, 0.5F); + if (tree.Length == 0) return; + DrawGizmos(ref draw, 0, 0); + } + + void DrawGizmos (ref CommandBuilder draw, int boxi, int depth) { + BBTreeBox box = tree[boxi]; + + var min = (Vector3) new Int3(box.rect.xmin, 0, box.rect.ymin); + var max = (Vector3) new Int3(box.rect.xmax, 0, box.rect.ymax); + + Vector3 center = (min+max)*0.5F; + Vector3 size = max-min; + + size = new Vector3(size.x, 1, size.z); + center.y += depth * 2; + + draw.xz.WireRectangle(center, new float2(size.x, size.z), AstarMath.IntToColor(depth, 1f)); + + if (!box.IsLeaf) { + DrawGizmos(ref draw, box.left, depth + 1); + DrawGizmos(ref draw, box.right, depth + 1); + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/BBTree.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/BBTree.cs.meta new file mode 100644 index 0000000..9ed7a3e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/BBTree.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 3a20480c673fd40a5bd2a4cc2206dbc4 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/ColliderMeshBuilder2D.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/ColliderMeshBuilder2D.cs new file mode 100644 index 0000000..98f433f --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/ColliderMeshBuilder2D.cs @@ -0,0 +1,336 @@ +using UnityEngine; +using System.Collections.Generic; +using Unity.Mathematics; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Burst; +using UnityEngine.Assertions; +using UnityEngine.Profiling; +using Pathfinding.Util; +using UnityEngine.Tilemaps; + + +namespace Pathfinding.Graphs.Navmesh { + [BurstCompile] + public struct CircleGeometryUtilities { + /// + /// Cached values for CircleRadiusAdjustmentFactor. + /// + /// We can calculate the area of a polygonized circle, and equate that with the area of a unit circle + /// + /// x * cos(math.PI / steps) * sin(math.PI/steps) * steps = pi + /// + /// Solving for the factor that makes them equal (x) gives the expression below. + /// + /// Generated using the python code: + /// + /// [math.sqrt(2 * math.pi / (i * math.sin(2 * math.pi / i))) for i in range(3, 23)] + /// + /// + /// It would be nice to generate this using a static constructor, but that is not supported by Unity's burst compiler. + /// + static readonly float[] circleRadiusAdjustmentFactors = new float[] { + 1.56f, 1.25f, 1.15f, 1.1f, 1.07f, 1.05f, 1.04f, 1.03f, 1.03f, 1.02f, 1.02f, 1.02f, 1.01f, 1.01f, 1.01f, 1.01f, 1.01f, 1.01f, 1.01f, 1.01f, + }; + + /// The number of steps required to get a circle with a maximum error of maxError + public static int CircleSteps (Matrix4x4 matrix, float radius, float maxError) { + // Take the maximum scale factor among the 3 axes. + // If the current matrix has a uniform scale then they are all the same. + var maxScaleFactor = math.sqrt(math.max(math.max(math.lengthsq((Vector3)matrix.GetColumn(0)), math.lengthsq((Vector3)matrix.GetColumn(1))), math.lengthsq((Vector3)matrix.GetColumn(2)))); + var realWorldRadius = radius * maxScaleFactor; + + // This expression is the first taylor expansion term of the formula below. + // It is almost identical to the formula below, but it avoids expensive trigonometric functions. + // return math.max(3, (int)math.ceil(math.PI * math.sqrt(realWorldRadius / (2*maxError)))); + var cosAngle = 1 - maxError / realWorldRadius; + int steps = math.max(3, (int)math.ceil(math.PI / math.acos(cosAngle))); + return steps; + } + + /// + /// Radius factor to adjust for circle approximation errors. + /// If a circle is approximated by fewer segments, it will be slightly smaller than the original circle. + /// This factor is used to adjust the radius of the circle so that the resulting circle will have roughly the same area as the original circle. + /// +#if MODULE_COLLECTIONS_2_0_0_OR_NEWER && UNITY_2022_2_OR_NEWER + [GenerateTestsForBurstCompatibility] +#endif + public static float CircleRadiusAdjustmentFactor (int steps) { + var index = steps - 3; + if (index < circleRadiusAdjustmentFactors.Length) { + if (index < 0) throw new System.ArgumentOutOfRangeException("Steps must be at least 3"); + return circleRadiusAdjustmentFactors[index]; + } else { + // Larger steps are approximately one + return 1; + } + } + } + + [BurstCompile] + public static class ColliderMeshBuilder2D { + public static int GenerateMeshesFromColliders (Collider2D[] colliders, int numColliders, float maxError, out NativeArray outputVertices, out NativeArray outputIndices, out NativeArray outputShapeMeshes) { + var group = new PhysicsShapeGroup2D(); + var shapeList = new NativeList(numColliders, Allocator.Temp); + var verticesList = new NativeList(numColliders*4, Allocator.Temp); + var matricesList = new NativeList(numColliders, Allocator.Temp); + var colliderIndexList = new NativeList(numColliders, Allocator.Temp); +#if ENABLE_UNITY_COLLECTIONS_CHECKS + var tempHandle = AtomicSafetyHandle.GetTempMemoryHandle(); +#endif + var handledRigidbodies = new HashSet(); + Profiler.BeginSample("GetShapes"); + + // Get the low level physics shapes from all colliders + var indexOffset = 0; + for (int i = 0; i < numColliders; i++) { + var coll = colliders[i]; + // Prevent errors from being logged when calling GetShapes on a collider that has no shapes + if (coll == null || coll.shapeCount == 0) continue; + + var rigid = coll.attachedRigidbody; + int shapeCount; + if (rigid == null) { + if (coll is TilemapCollider2D tilemapColl) { + // Ensure the tilemap is up to date + tilemapColl.ProcessTilemapChanges(); + } + shapeCount = coll.GetShapes(group); + } else if (handledRigidbodies.Add(rigid)) { + // Trying to get the shapes from a collider that is attached to a rigidbody will log annoying errors (this seems like a Unity bug tbh), + // so we must call GetShapes on the rigidbody instead. + shapeCount = rigid.GetShapes(group); + } else { + continue; + } + shapeList.Length += shapeCount; + verticesList.Length += group.vertexCount; + var subShapes = shapeList.AsArray().GetSubArray(shapeList.Length - shapeCount, shapeCount); + var subVertices = verticesList.AsArray().GetSubArray(verticesList.Length - group.vertexCount, group.vertexCount); + // Using AsArray and then GetSubArray will create an invalid safety handle due to unity limitations. + // We work around this by setting the safety handle to a temporary handle. +#if ENABLE_UNITY_COLLECTIONS_CHECKS + NativeArrayUnsafeUtility.SetAtomicSafetyHandle(ref subShapes, tempHandle); + NativeArrayUnsafeUtility.SetAtomicSafetyHandle(ref subVertices, tempHandle); +#endif + group.GetShapeData(subShapes, subVertices); + for (int j = 0; j < shapeCount; j++) { + var shape = subShapes[j]; + shape.vertexStartIndex += indexOffset; + subShapes[j] = shape; + } + indexOffset += subVertices.Length; + matricesList.AddReplicate(group.localToWorldMatrix, shapeCount); + colliderIndexList.AddReplicate(i, shapeCount); + } + Profiler.EndSample(); + Assert.AreEqual(shapeList.Length, matricesList.Length); + + Profiler.BeginSample("GenerateMeshes"); + var vertexBuffer = new NativeList(Allocator.Temp); + var indexBuffer = new NativeList(Allocator.Temp); + var shapeSpan = shapeList.AsUnsafeSpan(); + var verticesSpan = verticesList.AsUnsafeSpan().Reinterpret(); + var matricesSpan = matricesList.AsUnsafeSpan(); + var indexSpan = colliderIndexList.AsUnsafeSpan(); + outputShapeMeshes = new NativeArray(shapeList.Length, Allocator.Persistent); + var outputShapeMeshesSpan = outputShapeMeshes.AsUnsafeSpan(); + int outputMeshCount; + unsafe { + outputMeshCount = GenerateMeshesFromShapes( + ref shapeSpan, + ref verticesSpan, + ref matricesSpan, + ref indexSpan, + ref UnsafeUtility.AsRef >(vertexBuffer.GetUnsafeList()), + ref UnsafeUtility.AsRef >(indexBuffer.GetUnsafeList()), + ref outputShapeMeshesSpan, + maxError + ); + } + + Profiler.EndSample(); + Profiler.BeginSample("Copy"); + outputVertices = vertexBuffer.ToArray(Allocator.Persistent); + outputIndices = new NativeArray(indexBuffer.AsArray().Reinterpret(12), Allocator.Persistent); + Profiler.EndSample(); + return outputMeshCount; + } + + public struct ShapeMesh { + public Matrix4x4 matrix; + public Bounds bounds; + public int startIndex; + public int endIndex; + public int tag; + } + + static void AddCapsuleMesh (float2 c1, float2 c2, ref Matrix4x4 shapeMatrix, float radius, float maxError, ref UnsafeList outputVertices, ref UnsafeList outputIndices, ref float3 mn, ref float3 mx) { + var steps = math.max(4, CircleGeometryUtilities.CircleSteps(shapeMatrix, radius, maxError)); + // We are only generating a semicircle at a time, so reduce the number of steps + steps = (steps / 2) + 1; + radius = radius * CircleGeometryUtilities.CircleRadiusAdjustmentFactor(2*(steps-1)); + + var center1 = new Vector3(c1.x, c1.y, 0); + var center2 = new Vector3(c2.x, c2.y, 0); + var axis = math.normalizesafe(c2 - c1); + var crossAxis = new float2(-axis.y, axis.x); + var dx = radius * new Vector3(crossAxis.x, crossAxis.y, 0); + var dy = radius * new Vector3(axis.x, axis.y, 0); + var angle = math.PI / (steps-1); + + var startVertex = outputVertices.Length; + var startVertex2 = startVertex + steps; + outputVertices.Length += steps*2; + for (int j = 0; j < steps; j++) { + math.sincos(angle * j, out var sin, out var cos); + + // Generate first semi-circle + var p = center1 + cos * dx - sin * dy; + mn = math.min(mn, p); + mx = math.max(mx, p); + outputVertices[startVertex + j] = p; + + // Generate second semi-circle + p = center2 - cos * dx + sin * dy; + mn = math.min(mn, p); + mx = math.max(mx, p); + outputVertices[startVertex2 + j] = p; + } + var startIndex = outputIndices.Length; + var startIndex2 = startIndex + steps-2; + outputIndices.Length += (steps-2)*2; + for (int j = 1; j < steps - 1; j++) { + // Triangle for first semi-circle + outputIndices[startIndex + j - 1] = new int3(startVertex, startVertex + j, startVertex + j + 1); + // Triangle for second semi-circle + outputIndices[startIndex2 + j - 1] = new int3(startVertex2, startVertex2 + j, startVertex2 + j + 1); + } + + // Generate the connection between the two semi-circles + outputIndices.Add(new int3(startVertex, startVertex + steps - 1, startVertex2)); + outputIndices.Add(new int3(startVertex, startVertex2, startVertex2 + steps - 1)); + } + + [BurstCompile] + public static int GenerateMeshesFromShapes ( + ref UnsafeSpan shapes, + ref UnsafeSpan vertices, + ref UnsafeSpan shapeMatrices, + ref UnsafeSpan groupIndices, + ref UnsafeList outputVertices, + ref UnsafeList outputIndices, + ref UnsafeSpan outputShapeMeshes, + float maxError + ) { + var groupStartIndex = 0; + var mn = new float3(float.MaxValue, float.MaxValue, float.MaxValue); + var mx = new float3(float.MinValue, float.MinValue, float.MinValue); + int outputMeshIndex = 0; + for (int i = 0; i < shapes.Length; i++) { + var shape = shapes[i]; + var shapeVertices = vertices.Slice(shape.vertexStartIndex, shape.vertexCount); + var shapeMatrix = shapeMatrices[i]; + switch (shape.shapeType) { + case PhysicsShapeType2D.Circle: { + var steps = CircleGeometryUtilities.CircleSteps(shapeMatrix, shape.radius, maxError); + var radius = shape.radius * CircleGeometryUtilities.CircleRadiusAdjustmentFactor(steps); + var center = new Vector3(shapeVertices[0].x, shapeVertices[0].y, 0); + var d1 = new Vector3(radius, 0, 0); + var d2 = new Vector3(0, radius, 0); + var angle = 2 * math.PI / steps; + var startVertex = outputVertices.Length; + for (int j = 0; j < steps; j++) { + math.sincos(angle * j, out var sin, out var cos); + var p = center + cos * d1 + sin * d2; + mn = math.min(mn, p); + mx = math.max(mx, p); + outputVertices.Add(p); + } + for (int j = 1; j < steps; j++) { + outputIndices.Add(new int3(startVertex, startVertex + j, startVertex + (j + 1) % steps)); + } + break; + } + case PhysicsShapeType2D.Capsule: { + var c1 = shapeVertices[0]; + var c2 = shapeVertices[1]; + AddCapsuleMesh(c1, c2, ref shapeMatrix, shape.radius, maxError, ref outputVertices, ref outputIndices, ref mn, ref mx); + break; + } + case PhysicsShapeType2D.Polygon: { + var startVertex = outputVertices.Length; + outputVertices.Resize(startVertex + shape.vertexCount, NativeArrayOptions.UninitializedMemory); + for (int j = 0; j < shape.vertexCount; j++) { + var p = new Vector3(shapeVertices[j].x, shapeVertices[j].y, 0); + mn = math.min(mn, p); + mx = math.max(mx, p); + outputVertices[startVertex + j] = p; + } + outputIndices.SetCapacity(math.ceilpow2(outputIndices.Length + (shape.vertexCount - 2))); + for (int j = 1; j < shape.vertexCount - 1; j++) { + outputIndices.AddNoResize(new int3(startVertex, startVertex + j, startVertex + j + 1)); + } + break; + } + case PhysicsShapeType2D.Edges: { + if (shape.radius > maxError) { + for (int j = 0; j < shape.vertexCount - 1; j++) { + AddCapsuleMesh(shapeVertices[j], shapeVertices[j+1], ref shapeMatrix, shape.radius, maxError, ref outputVertices, ref outputIndices, ref mn, ref mx); + } + } else { + var startVertex = outputVertices.Length; + outputVertices.Resize(startVertex + shape.vertexCount, NativeArrayOptions.UninitializedMemory); + for (int j = 0; j < shape.vertexCount; j++) { + var p = new Vector3(shapeVertices[j].x, shapeVertices[j].y, 0); + mn = math.min(mn, p); + mx = math.max(mx, p); + outputVertices[startVertex + j] = p; + } + outputIndices.SetCapacity(math.ceilpow2(outputIndices.Length + (shape.vertexCount - 1))); + for (int j = 0; j < shape.vertexCount - 1; j++) { + // An edge is represented by a degenerate triangle + outputIndices.AddNoResize(new int3(startVertex + j, startVertex + j + 1, startVertex + j + 1)); + } + } + break; + } + default: + throw new System.Exception("Unexpected PhysicsShapeType2D"); + } + + // Merge shapes which are in the same group into a single ShapeMesh struct. + // This is done to reduce the per-shape overhead a bit. + // Don't do it too much, though, since that can cause filtering to not work too well. + // For example if a recast graph recalculates a single tile in a 2D scene, we don't want to include the whole collider for the + // TilemapCollider2D in the scene when doing rasterization, only the shapes around the tile that is recalculated. + // We will still process the whole TilemapCollider2D (no way around that), but we want to be able to exclude shapes shapes as quickly as possible + // based on their bounding boxes. + const int DesiredTrianglesPerGroup = 100; + if (i == shapes.Length - 1 || groupIndices[i] != groupIndices[i+1] || outputIndices.Length - groupStartIndex > DesiredTrianglesPerGroup) { + // Transform the bounding box to world space + // This is not the tightest bounding box, but it is good enough + var m = new ToWorldMatrix(new float3x3((float4x4)shapeMatrix)); + var bounds = new Bounds((mn + mx)*0.5f, mx - mn); + bounds = m.ToWorld(bounds); + bounds.center += (Vector3)shapeMatrix.GetColumn(3); + + outputShapeMeshes[outputMeshIndex++] = new ShapeMesh { + bounds = bounds, + matrix = shapeMatrix, + startIndex = groupStartIndex * 3, + endIndex = outputIndices.Length * 3, + tag = groupIndices[i] + }; + + mn = new float3(float.MaxValue, float.MaxValue, float.MaxValue); + mx = new float3(float.MinValue, float.MinValue, float.MinValue); + groupStartIndex = outputIndices.Length; + } + } + + return outputMeshIndex; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/ColliderMeshBuilder2D.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/ColliderMeshBuilder2D.cs.meta new file mode 100644 index 0000000..12bd06e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/ColliderMeshBuilder2D.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 65d5806c4978b7e46b69297ca838f91c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs.meta new file mode 100644 index 0000000..6867f71 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 7d87dc471eec3ae4dac67ee232391350 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildNodes.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildNodes.cs new file mode 100644 index 0000000..841c86d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildNodes.cs @@ -0,0 +1,92 @@ +using Pathfinding.Jobs; +using Pathfinding.Util; +using Unity.Collections; +using Unity.Jobs; +using UnityEngine; +using UnityEngine.Profiling; + +namespace Pathfinding.Graphs.Navmesh.Jobs { + /// + /// Builds nodes and tiles and prepares them for pathfinding. + /// + /// Takes input from a job and outputs a . + /// + /// This job takes the following steps: + /// - Calculate connections between nodes inside each tile + /// - Create node and tile objects + /// - Connect adjacent tiles together + /// + public struct JobBuildNodes { + AstarPath astar; + uint graphIndex; + public uint initialPenalty; + public bool recalculateNormals; + public float maxTileConnectionEdgeDistance; + Matrix4x4 graphToWorldSpace; + TileLayout tileLayout; + + public class BuildNodeTilesOutput : IProgress, System.IDisposable { + public TileBuilder.TileBuilderOutput dependency; + public NavmeshTile[] tiles; + + public float Progress => dependency.Progress; + + public void Dispose () { + } + } + + internal JobBuildNodes(RecastGraph graph, TileLayout tileLayout) { + this.astar = graph.active; + this.tileLayout = tileLayout; + this.graphIndex = graph.graphIndex; + this.initialPenalty = graph.initialPenalty; + this.recalculateNormals = graph.RecalculateNormals; + this.maxTileConnectionEdgeDistance = graph.MaxTileConnectionEdgeDistance; + this.graphToWorldSpace = tileLayout.transform.matrix; + } + + public Promise Schedule (DisposeArena arena, Promise dependency) { + var input = dependency.GetValue(); + var tileRect = input.tileMeshes.tileRect; + UnityEngine.Assertions.Assert.AreEqual(input.tileMeshes.tileMeshes.Length, tileRect.Area); + var tiles = new NavmeshTile[tileRect.Area]; + var tilesGCHandle = System.Runtime.InteropServices.GCHandle.Alloc(tiles); + var nodeConnections = new NativeArray(tileRect.Area, Allocator.Persistent); + + var calculateConnectionsJob = new JobCalculateTriangleConnections { + tileMeshes = input.tileMeshes.tileMeshes, + nodeConnections = nodeConnections, + }.Schedule(dependency.handle); + + var tileWorldSize = new Vector2(tileLayout.TileWorldSizeX, tileLayout.TileWorldSizeZ); + var createTilesJob = new JobCreateTiles { + tileMeshes = input.tileMeshes.tileMeshes, + tiles = tilesGCHandle, + tileRect = tileRect, + graphTileCount = tileLayout.tileCount, + graphIndex = graphIndex, + initialPenalty = initialPenalty, + recalculateNormals = recalculateNormals, + graphToWorldSpace = this.graphToWorldSpace, + tileWorldSize = tileWorldSize, + }.Schedule(dependency.handle); + + var applyConnectionsJob = new JobWriteNodeConnections { + nodeConnections = nodeConnections, + tiles = tilesGCHandle, + }.Schedule(JobHandle.CombineDependencies(calculateConnectionsJob, createTilesJob)); + + Profiler.BeginSample("Scheduling ConnectTiles"); + var connectTilesDependency = JobConnectTiles.ScheduleBatch(tilesGCHandle, applyConnectionsJob, tileRect, tileWorldSize, maxTileConnectionEdgeDistance); + Profiler.EndSample(); + + arena.Add(tilesGCHandle); + arena.Add(nodeConnections); + + return new Promise(connectTilesDependency, new BuildNodeTilesOutput { + dependency = input, + tiles = tiles, + }); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildNodes.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildNodes.cs.meta new file mode 100644 index 0000000..c8446f5 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildNodes.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: bd51eca97d285874d997d22edd420a27 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVertices.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVertices.cs new file mode 100644 index 0000000..f68ed2b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVertices.cs @@ -0,0 +1,105 @@ +using Pathfinding.Util; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using UnityEngine; + +namespace Pathfinding.Graphs.Navmesh.Jobs { + /// + /// Builds tiles from raw mesh vertices and indices. + /// + /// This job takes the following steps: + /// - Transform all vertices using the matrix. + /// - Remove duplicate vertices + /// - If is enabled: ensure all triangles are laid out in the clockwise direction. + /// + [BurstCompile(FloatMode = FloatMode.Default)] + public struct JobBuildTileMeshFromVertices : IJob { + public NativeArray vertices; + public NativeArray indices; + public Matrix4x4 meshToGraph; + public NativeArray outputBuffers; + public bool recalculateNormals; + + + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobTransformTileCoordinates : IJob { + public NativeArray vertices; + public NativeArray outputVertices; + public Matrix4x4 matrix; + + public void Execute () { + for (int i = 0; i < vertices.Length; i++) { + outputVertices[i] = (Int3)matrix.MultiplyPoint3x4(vertices[i]); + } + } + } + + public struct BuildNavmeshOutput : IProgress, System.IDisposable { + public NativeArray tiles; + + public float Progress => 0.0f; + + public void Dispose () { + for (int i = 0; i < tiles.Length; i++) tiles[i].Dispose(); + tiles.Dispose(); + } + } + + public static Promise Schedule (NativeArray vertices, NativeArray indices, Matrix4x4 meshToGraph, bool recalculateNormals) { + if (vertices.Length > NavmeshBase.VertexIndexMask) throw new System.ArgumentException("Too many vertices in the navmesh graph. Provided " + vertices.Length + ", but the maximum number of vertices per tile is " + NavmeshBase.VertexIndexMask + ". You can raise this limit by enabling ASTAR_RECAST_LARGER_TILES in the A* Inspector Optimizations tab"); + + var outputBuffers = new NativeArray(1, Allocator.Persistent); + + var job = new JobBuildTileMeshFromVertices { + vertices = vertices, + indices = indices, + meshToGraph = meshToGraph, + outputBuffers = outputBuffers, + recalculateNormals = recalculateNormals, + }.Schedule(); + return new Promise(job, new BuildNavmeshOutput { + tiles = outputBuffers, + }); + } + + public void Execute () { + var int3vertices = new NativeArray(vertices.Length, Allocator.Temp); + var tags = new NativeArray(indices.Length / 3, Allocator.Temp, NativeArrayOptions.ClearMemory); + + new JobTransformTileCoordinates { + vertices = vertices, + outputVertices = int3vertices, + matrix = meshToGraph, + }.Execute(); + + unsafe { + UnityEngine.Assertions.Assert.IsTrue(this.outputBuffers.Length == 1); + var tile = (TileMesh.TileMeshUnsafe*) this.outputBuffers.GetUnsafePtr(); + var outputVertices = &tile->verticesInTileSpace; + var outputTriangles = &tile->triangles; + var outputTags = &tile->tags; + *outputVertices = new UnsafeAppendBuffer(0, 4, Allocator.Persistent); + *outputTriangles = new UnsafeAppendBuffer(0, 4, Allocator.Persistent); + *outputTags = new UnsafeAppendBuffer(0, 4, Allocator.Persistent); + new MeshUtility.JobRemoveDuplicateVertices { + vertices = int3vertices, + triangles = indices, + tags = tags, + outputVertices = outputVertices, + outputTriangles = outputTriangles, + outputTags = outputTags, + }.Execute(); + + if (recalculateNormals) { + var verticesSpan = outputVertices->AsUnsafeSpan(); + var trianglesSpan = outputTriangles->AsUnsafeSpan(); + MeshUtility.MakeTrianglesClockwise(ref verticesSpan, ref trianglesSpan); + } + } + + int3vertices.Dispose(); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVertices.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVertices.cs.meta new file mode 100644 index 0000000..b7f6886 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVertices.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a22b53fa064d9344988e2a86b73851b1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVoxels.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVoxels.cs new file mode 100644 index 0000000..a6ca868 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVoxels.cs @@ -0,0 +1,288 @@ +using Pathfinding.Jobs; +using Pathfinding.Util; +using Pathfinding.Graphs.Navmesh.Voxelization.Burst; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using UnityEngine; +using Unity.Profiling; + +namespace Pathfinding.Graphs.Navmesh.Jobs { + /// + /// Scratch space for building navmesh tiles using voxelization. + /// + /// This uses quite a lot of memory, so it is used by a single worker thread for multiple tiles in order to minimize allocations. + /// + public struct TileBuilderBurst : IArenaDisposable { + public LinkedVoxelField linkedVoxelField; + public CompactVoxelField compactVoxelField; + public NativeList distanceField; + public NativeQueue tmpQueue1; + public NativeQueue tmpQueue2; + public NativeList contours; + public NativeList contourVertices; + public VoxelMesh voxelMesh; + + public TileBuilderBurst (int width, int depth, int voxelWalkableHeight, int maximumVoxelYCoord) { + linkedVoxelField = new LinkedVoxelField(width, depth, maximumVoxelYCoord); + compactVoxelField = new CompactVoxelField(width, depth, voxelWalkableHeight, Allocator.Persistent); + tmpQueue1 = new NativeQueue(Allocator.Persistent); + tmpQueue2 = new NativeQueue(Allocator.Persistent); + distanceField = new NativeList(0, Allocator.Persistent); + contours = new NativeList(Allocator.Persistent); + contourVertices = new NativeList(Allocator.Persistent); + voxelMesh = new VoxelMesh { + verts = new NativeList(Allocator.Persistent), + tris = new NativeList(Allocator.Persistent), + areas = new NativeList(Allocator.Persistent), + }; + } + + void IArenaDisposable.DisposeWith (DisposeArena arena) { + arena.Add(linkedVoxelField); + arena.Add(compactVoxelField); + arena.Add(distanceField); + arena.Add(tmpQueue1); + arena.Add(tmpQueue2); + arena.Add(contours); + arena.Add(contourVertices); + arena.Add(voxelMesh); + } + } + + /// + /// Builds tiles from a polygon soup using voxelization. + /// + /// This job takes the following steps: + /// - Voxelize the input meshes + /// - Filter and process the resulting voxelization in various ways to remove unwanted artifacts and make it better suited for pathfinding. + /// - Extract a walkable surface from the voxelization. + /// - Triangulate this surface and create navmesh tiles from it. + /// + /// This job uses work stealing to distribute the work between threads. The communication happens using a shared queue and the atomic variable. + /// + [BurstCompile(CompileSynchronously = true)] + // TODO: [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobBuildTileMeshFromVoxels : IJob { + public TileBuilderBurst tileBuilder; + [ReadOnly] + public TileBuilder.BucketMapping inputMeshes; + [ReadOnly] + public NativeArray tileGraphSpaceBounds; + public Matrix4x4 voxelToTileSpace; + + /// + /// Limits of the graph space bounds for the whole graph on the XZ plane. + /// + /// Used to crop the border tiles to exactly the limits of the graph's bounding box. + /// + public Vector2 graphSpaceLimits; + + [NativeDisableUnsafePtrRestriction] + public unsafe TileMesh.TileMeshUnsafe* outputMeshes; + + /// Max number of tiles to process in this job + public int maxTiles; + + public int voxelWalkableClimb; + public uint voxelWalkableHeight; + public float cellSize; + public float cellHeight; + public float maxSlope; + public RecastGraph.DimensionMode dimensionMode; + public RecastGraph.BackgroundTraversability backgroundTraversability; + public Matrix4x4 graphToWorldSpace; + public int characterRadiusInVoxels; + public int tileBorderSizeInVoxels; + public int minRegionSize; + public float maxEdgeLength; + public float contourMaxError; + [ReadOnly] + public NativeArray relevantGraphSurfaces; + public RecastGraph.RelevantGraphSurfaceMode relevantGraphSurfaceMode; + + [NativeDisableUnsafePtrRestriction] + public unsafe int* currentTileCounter; + + public void SetOutputMeshes (NativeArray arr) { + unsafe { + outputMeshes = (TileMesh.TileMeshUnsafe*)arr.GetUnsafeReadOnlyPtr(); + } + } + + public void SetCounter (NativeReference counter) { + unsafe { + // Note: The pointer cast is only necessary when using early versions of the collections package. + currentTileCounter = (int*)counter.GetUnsafePtr(); + } + } + + private static readonly ProfilerMarker MarkerVoxelize = new ProfilerMarker("Voxelize"); + private static readonly ProfilerMarker MarkerFilterLedges = new ProfilerMarker("FilterLedges"); + private static readonly ProfilerMarker MarkerFilterLowHeightSpans = new ProfilerMarker("FilterLowHeightSpans"); + private static readonly ProfilerMarker MarkerBuildCompactField = new ProfilerMarker("BuildCompactField"); + private static readonly ProfilerMarker MarkerBuildConnections = new ProfilerMarker("BuildConnections"); + private static readonly ProfilerMarker MarkerErodeWalkableArea = new ProfilerMarker("ErodeWalkableArea"); + private static readonly ProfilerMarker MarkerBuildDistanceField = new ProfilerMarker("BuildDistanceField"); + private static readonly ProfilerMarker MarkerBuildRegions = new ProfilerMarker("BuildRegions"); + private static readonly ProfilerMarker MarkerBuildContours = new ProfilerMarker("BuildContours"); + private static readonly ProfilerMarker MarkerBuildMesh = new ProfilerMarker("BuildMesh"); + private static readonly ProfilerMarker MarkerConvertAreasToTags = new ProfilerMarker("ConvertAreasToTags"); + private static readonly ProfilerMarker MarkerRemoveDuplicateVertices = new ProfilerMarker("RemoveDuplicateVertices"); + private static readonly ProfilerMarker MarkerTransformTileCoordinates = new ProfilerMarker("TransformTileCoordinates"); + + public void Execute () { + for (int k = 0; k < maxTiles; k++) { + // Grab the next tile index that we should calculate + int i; + unsafe { + i = System.Threading.Interlocked.Increment(ref UnsafeUtility.AsRef(currentTileCounter)) - 1; + } + if (i >= tileGraphSpaceBounds.Length) return; + + tileBuilder.linkedVoxelField.ResetLinkedVoxelSpans(); + if (dimensionMode == RecastGraph.DimensionMode.Dimension2D && backgroundTraversability == RecastGraph.BackgroundTraversability.Walkable) { + tileBuilder.linkedVoxelField.SetWalkableBackground(); + } + + var bucketStart = i > 0 ? inputMeshes.bucketRanges[i-1] : 0; + var bucketEnd = inputMeshes.bucketRanges[i]; + MarkerVoxelize.Begin(); + new JobVoxelize { + inputMeshes = inputMeshes.meshes, + bucket = inputMeshes.pointers.GetSubArray(bucketStart, bucketEnd - bucketStart), + voxelWalkableClimb = voxelWalkableClimb, + voxelWalkableHeight = voxelWalkableHeight, + cellSize = cellSize, + cellHeight = cellHeight, + maxSlope = maxSlope, + graphTransform = graphToWorldSpace, + graphSpaceBounds = tileGraphSpaceBounds[i], + graphSpaceLimits = graphSpaceLimits, + voxelArea = tileBuilder.linkedVoxelField, + }.Execute(); + MarkerVoxelize.End(); + + + + MarkerFilterLedges.Begin(); + new JobFilterLedges { + field = tileBuilder.linkedVoxelField, + voxelWalkableClimb = voxelWalkableClimb, + voxelWalkableHeight = voxelWalkableHeight, + cellSize = cellSize, + cellHeight = cellHeight, + }.Execute(); + MarkerFilterLedges.End(); + + MarkerFilterLowHeightSpans.Begin(); + new JobFilterLowHeightSpans { + field = tileBuilder.linkedVoxelField, + voxelWalkableHeight = voxelWalkableHeight, + }.Execute(); + MarkerFilterLowHeightSpans.End(); + + MarkerBuildCompactField.Begin(); + new JobBuildCompactField { + input = tileBuilder.linkedVoxelField, + output = tileBuilder.compactVoxelField, + }.Execute(); + MarkerBuildCompactField.End(); + + MarkerBuildConnections.Begin(); + new JobBuildConnections { + field = tileBuilder.compactVoxelField, + voxelWalkableHeight = (int)voxelWalkableHeight, + voxelWalkableClimb = voxelWalkableClimb, + }.Execute(); + MarkerBuildConnections.End(); + + MarkerErodeWalkableArea.Begin(); + new JobErodeWalkableArea { + field = tileBuilder.compactVoxelField, + radius = characterRadiusInVoxels, + }.Execute(); + MarkerErodeWalkableArea.End(); + + MarkerBuildDistanceField.Begin(); + new JobBuildDistanceField { + field = tileBuilder.compactVoxelField, + output = tileBuilder.distanceField, + }.Execute(); + MarkerBuildDistanceField.End(); + + MarkerBuildRegions.Begin(); + new JobBuildRegions { + field = tileBuilder.compactVoxelField, + distanceField = tileBuilder.distanceField, + borderSize = tileBorderSizeInVoxels, + minRegionSize = Mathf.RoundToInt(minRegionSize), + srcQue = tileBuilder.tmpQueue1, + dstQue = tileBuilder.tmpQueue2, + relevantGraphSurfaces = relevantGraphSurfaces, + relevantGraphSurfaceMode = relevantGraphSurfaceMode, + cellSize = cellSize, + cellHeight = cellHeight, + graphTransform = graphToWorldSpace, + graphSpaceBounds = tileGraphSpaceBounds[i], + }.Execute(); + MarkerBuildRegions.End(); + + MarkerBuildContours.Begin(); + new JobBuildContours { + field = tileBuilder.compactVoxelField, + maxError = contourMaxError, + maxEdgeLength = maxEdgeLength, + buildFlags = VoxelUtilityBurst.RC_CONTOUR_TESS_WALL_EDGES | VoxelUtilityBurst.RC_CONTOUR_TESS_TILE_EDGES, + cellSize = cellSize, + outputContours = tileBuilder.contours, + outputVerts = tileBuilder.contourVertices, + }.Execute(); + MarkerBuildContours.End(); + + MarkerBuildMesh.Begin(); + new JobBuildMesh { + contours = tileBuilder.contours, + contourVertices = tileBuilder.contourVertices, + mesh = tileBuilder.voxelMesh, + field = tileBuilder.compactVoxelField, + }.Execute(); + MarkerBuildMesh.End(); + + unsafe { + TileMesh.TileMeshUnsafe* outputTileMesh = outputMeshes + i; + *outputTileMesh = new TileMesh.TileMeshUnsafe { + verticesInTileSpace = new UnsafeAppendBuffer(0, 4, Allocator.Persistent), + triangles = new UnsafeAppendBuffer(0, 4, Allocator.Persistent), + tags = new UnsafeAppendBuffer(0, 4, Allocator.Persistent), + }; + + MarkerConvertAreasToTags.Begin(); + new JobConvertAreasToTags { + areas = tileBuilder.voxelMesh.areas, + }.Execute(); + MarkerConvertAreasToTags.End(); + + MarkerRemoveDuplicateVertices.Begin(); + new MeshUtility.JobRemoveDuplicateVertices { + vertices = tileBuilder.voxelMesh.verts.AsArray(), + triangles = tileBuilder.voxelMesh.tris.AsArray(), + tags = tileBuilder.voxelMesh.areas.AsArray(), + outputTags = &outputTileMesh->tags, + outputVertices = &outputTileMesh->verticesInTileSpace, + outputTriangles = &outputTileMesh->triangles, + }.Execute(); + MarkerRemoveDuplicateVertices.End(); + + MarkerTransformTileCoordinates.Begin(); + new JobTransformTileCoordinates { + vertices = &outputTileMesh->verticesInTileSpace, + matrix = voxelToTileSpace, + }.Execute(); + MarkerTransformTileCoordinates.End(); + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVoxels.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVoxels.cs.meta new file mode 100644 index 0000000..4e77298 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobBuildTileMeshFromVoxels.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 20aeb827260a74a4492e7687fdebb14f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCalculateTriangleConnections.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCalculateTriangleConnections.cs new file mode 100644 index 0000000..b0da1ed --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCalculateTriangleConnections.cs @@ -0,0 +1,73 @@ +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Navmesh.Jobs { + /// + /// Calculates node connections between triangles within each tile. + /// Connections between tiles are handled at a later stage in . + /// + [BurstCompile] + public struct JobCalculateTriangleConnections : IJob { + [ReadOnly] + public NativeArray tileMeshes; + [WriteOnly] + public NativeArray nodeConnections; + + public struct TileNodeConnectionsUnsafe { + /// Stream of packed connection edge infos (from ) + public Unity.Collections.LowLevel.Unsafe.UnsafeAppendBuffer neighbours; + /// Number of neighbours for each triangle + public Unity.Collections.LowLevel.Unsafe.UnsafeAppendBuffer neighbourCounts; + } + + public void Execute () { + Assert.AreEqual(tileMeshes.Length, nodeConnections.Length); + + var nodeRefs = new NativeParallelHashMap(128, Allocator.Temp); + bool duplicates = false; + for (int ti = 0; ti < tileMeshes.Length; ti++) { + nodeRefs.Clear(); + var tile = tileMeshes[ti]; + var numIndices = tile.triangles.Length / sizeof(int); + var neighbours = new Unity.Collections.LowLevel.Unsafe.UnsafeAppendBuffer(numIndices * 2 * 4, 4, Allocator.Persistent); + var neighbourCounts = new Unity.Collections.LowLevel.Unsafe.UnsafeAppendBuffer(numIndices * 4, 4, Allocator.Persistent); + const int TriangleIndexBits = 28; + unsafe { + Assert.IsTrue(numIndices % 3 == 0); + var triangles = (int*)tile.triangles.Ptr; + for (int i = 0, j = 0; i < numIndices; i += 3, j++) { + duplicates |= !nodeRefs.TryAdd(new int2(triangles[i+0], triangles[i+1]), (uint)j | (0 << TriangleIndexBits)); + duplicates |= !nodeRefs.TryAdd(new int2(triangles[i+1], triangles[i+2]), (uint)j | (1 << TriangleIndexBits)); + duplicates |= !nodeRefs.TryAdd(new int2(triangles[i+2], triangles[i+0]), (uint)j | (2 << TriangleIndexBits)); + } + + for (int i = 0; i < numIndices; i += 3) { + var cnt = 0; + for (int edge = 0; edge < 3; edge++) { + if (nodeRefs.TryGetValue(new int2(triangles[i+((edge+1) % 3)], triangles[i+edge]), out var match)) { + var other = match & ((1 << TriangleIndexBits) - 1); + var otherEdge = (int)(match >> TriangleIndexBits); + neighbours.Add(other); + var edgeInfo = Connection.PackShapeEdgeInfo((byte)edge, (byte)otherEdge, true, true, true); + neighbours.Add((int)edgeInfo); + cnt += 1; + } + } + neighbourCounts.Add(cnt); + } + } + nodeConnections[ti] = new TileNodeConnectionsUnsafe { + neighbours = neighbours, + neighbourCounts = neighbourCounts, + }; + } + + if (duplicates) { + UnityEngine.Debug.LogWarning("Duplicate triangle edges were found in the input mesh. These have been removed. Are you sure your mesh is suitable for being used as a navmesh directly?\nThis could be caused by the mesh's normals not being consistent."); + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCalculateTriangleConnections.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCalculateTriangleConnections.cs.meta new file mode 100644 index 0000000..4bc3c35 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCalculateTriangleConnections.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 30417132dbc15504abbdf1b70224c006 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConnectTiles.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConnectTiles.cs new file mode 100644 index 0000000..0782ecf --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConnectTiles.cs @@ -0,0 +1,159 @@ +using Unity.Collections; +using Unity.Jobs; +using Unity.Jobs.LowLevel.Unsafe; +using Unity.Mathematics; +using UnityEngine; + +namespace Pathfinding.Graphs.Navmesh.Jobs { + /// + /// Connects adjacent tiles together. + /// + /// This only creates connections between tiles. Connections internal to a tile should be handled by . + /// + /// Use the method to connect a bunch of tiles efficiently using maximum parallelism. + /// + public struct JobConnectTiles : IJob { + /// GCHandle referring to a NavmeshTile[] array of size tileRect.Width*tileRect.Height + public System.Runtime.InteropServices.GCHandle tiles; + public int coordinateSum; + public int direction; + public int zOffset; + public int zStride; + Vector2 tileWorldSize; + IntRect tileRect; + /// Maximum vertical distance between two tiles to create a connection between them + public float maxTileConnectionEdgeDistance; + + static readonly Unity.Profiling.ProfilerMarker ConnectTilesMarker = new Unity.Profiling.ProfilerMarker("ConnectTiles"); + + /// + /// Schedule jobs to connect all the given tiles with each other while exploiting as much parallelism as possible. + /// tilesHandle should be a GCHandle referring to a NavmeshTile[] array of size tileRect.Width*tileRect.Height. + /// + public static JobHandle ScheduleBatch (System.Runtime.InteropServices.GCHandle tilesHandle, JobHandle dependency, IntRect tileRect, Vector2 tileWorldSize, float maxTileConnectionEdgeDistance) { + // First connect all tiles with an EVEN coordinate sum + // This would be the white squares on a chess board. + // Then connect all tiles with an ODD coordinate sum (which would be all black squares on a chess board). + // This will prevent the different threads that do all + // this in parallel from conflicting with each other. + // The directions are also done separately + // first they are connected along the X direction and then along the Z direction. + // Looping over 0 and then 1 + + int workers = Mathf.Max(1, JobsUtility.JobWorkerCount); + var handles = new NativeArray(workers, Allocator.Temp); + for (int coordinateSum = 0; coordinateSum <= 1; coordinateSum++) { + for (int direction = 0; direction <= 1; direction++) { + for (int i = 0; i < workers; i++) { + handles[i] = new JobConnectTiles { + tiles = tilesHandle, + tileRect = tileRect, + tileWorldSize = tileWorldSize, + coordinateSum = coordinateSum, + direction = direction, + maxTileConnectionEdgeDistance = maxTileConnectionEdgeDistance, + zOffset = i, + zStride = workers, + }.Schedule(dependency); + } + dependency = JobHandle.CombineDependencies(handles); + } + } + + return dependency; + } + + /// + /// Schedule jobs to connect all the given tiles inside innerRect with tiles that are outside it, while exploiting as much parallelism as possible. + /// tilesHandle should be a GCHandle referring to a NavmeshTile[] array of size tileRect.Width*tileRect.Height. + /// + public static JobHandle ScheduleRecalculateBorders (System.Runtime.InteropServices.GCHandle tilesHandle, JobHandle dependency, IntRect tileRect, IntRect innerRect, Vector2 tileWorldSize, float maxTileConnectionEdgeDistance) { + var w = innerRect.Width; + var h = innerRect.Height; + + // Note: conservative estimate of number of handles. There may be fewer in reality. + var allDependencies = new NativeArray(2*w + 2*math.max(0, h - 2), Allocator.Temp); + int count = 0; + for (int z = 0; z < h; z++) { + for (int x = 0; x < w; x++) { + // Check if the tile is on the border of the inner rect + if (!(x == 0 || z == 0 || x == w - 1 || z == h - 1)) continue; + + var tileX = innerRect.xmin + x; + var tileZ = innerRect.ymin + z; + + // For a corner tile, the jobs need to run sequentially + var dep = dependency; + for (int direction = 0; direction < 4; direction++) { + var nx = tileX + (direction == 0 ? 1 : direction == 1 ? -1 : 0); + var nz = tileZ + (direction == 2 ? 1 : direction == 3 ? -1 : 0); + if (innerRect.Contains(nx, nz) || !tileRect.Contains(nx, nz)) { + continue; + } + + dep = new JobConnectTilesSingle { + tiles = tilesHandle, + tileIndex1 = tileX + tileZ * tileRect.Width, + tileIndex2 = nx + nz * tileRect.Width, + tileWorldSize = tileWorldSize, + maxTileConnectionEdgeDistance = maxTileConnectionEdgeDistance, + }.Schedule(dep); + } + + allDependencies[count++] = dep; + } + } + return JobHandle.CombineDependencies(allDependencies); + } + + public void Execute () { + var tiles = (NavmeshTile[])this.tiles.Target; + + var tileRectDepth = tileRect.Height; + var tileRectWidth = tileRect.Width; + for (int z = zOffset; z < tileRectDepth; z += zStride) { + for (int x = 0; x < tileRectWidth; x++) { + if ((x + z) % 2 == coordinateSum) { + int tileIndex1 = x + z * tileRectWidth; + int tileIndex2; + if (direction == 0 && x < tileRectWidth - 1) { + tileIndex2 = x + 1 + z * tileRectWidth; + } else if (direction == 1 && z < tileRectDepth - 1) { + tileIndex2 = x + (z + 1) * tileRectWidth; + } else { + continue; + } + + ConnectTilesMarker.Begin(); + NavmeshBase.ConnectTiles(tiles[tileIndex1], tiles[tileIndex2], tileWorldSize.x, tileWorldSize.y, maxTileConnectionEdgeDistance); + ConnectTilesMarker.End(); + } + } + } + } + } + + /// + /// Connects two adjacent tiles together. + /// + /// This only creates connections between tiles. Connections internal to a tile should be handled by . + /// + struct JobConnectTilesSingle : IJob { + /// GCHandle referring to a NavmeshTile[] array of size tileRect.Width*tileRect.Height + public System.Runtime.InteropServices.GCHandle tiles; + /// Index of the first tile in the array + public int tileIndex1; + /// Index of the second tile in the array + public int tileIndex2; + /// Size of a tile in world units + public Vector2 tileWorldSize; + /// Maximum vertical distance between two tiles to create a connection between them + public float maxTileConnectionEdgeDistance; + + public void Execute () { + var tiles = (NavmeshTile[])this.tiles.Target; + + NavmeshBase.ConnectTiles(tiles[tileIndex1], tiles[tileIndex2], tileWorldSize.x, tileWorldSize.y, maxTileConnectionEdgeDistance); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConnectTiles.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConnectTiles.cs.meta new file mode 100644 index 0000000..766f092 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConnectTiles.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: dd00a18824d04764783722c547fb60f7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConvertAreasToTags.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConvertAreasToTags.cs new file mode 100644 index 0000000..2197d9c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConvertAreasToTags.cs @@ -0,0 +1,23 @@ +using Pathfinding.Graphs.Navmesh.Voxelization.Burst; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; + +namespace Pathfinding.Graphs.Navmesh.Jobs { + /// Convert recast region IDs to the tags that should be applied to the nodes + [BurstCompile] + public struct JobConvertAreasToTags : IJob { + public NativeList areas; + + public void Execute () { + unsafe { + for (int i = 0; i < areas.Length; i++) { + var area = areas[i]; + // The user supplied IDs start at 1 because 0 is reserved for NotWalkable + areas[i] = (area & VoxelUtilityBurst.TagReg) != 0 ? (area & VoxelUtilityBurst.TagRegMask) - 1 : 0; + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConvertAreasToTags.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConvertAreasToTags.cs.meta new file mode 100644 index 0000000..3b4daad --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobConvertAreasToTags.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 229fdb01207c1ab4796deea78744e136 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCreateTiles.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCreateTiles.cs new file mode 100644 index 0000000..44368b3 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCreateTiles.cs @@ -0,0 +1,115 @@ +using Pathfinding.Util; +using Unity.Collections; +using Unity.Jobs; +using UnityEngine; +using UnityEngine.Assertions; +using UnityEngine.Profiling; + +namespace Pathfinding.Graphs.Navmesh.Jobs { + /// + /// Builds tiles optimized for pathfinding, from a list of . + /// + /// This job takes the following steps: + /// - Transform all vertices using the matrix. + /// - Remove duplicate vertices + /// - If is enabled: ensure all triangles are laid out in the clockwise direction. + /// + public struct JobCreateTiles : IJob { + /// An array of of length tileRect.Width*tileRect.Height + [ReadOnly] + public NativeArray tileMeshes; + + /// + /// An array of of length tileRect.Width*tileRect.Height. + /// This array will be filled with the created tiles. + /// + public System.Runtime.InteropServices.GCHandle tiles; + + /// Graph index of the graph that these nodes will be added to + public uint graphIndex; + + /// + /// Number of tiles in the graph. + /// + /// This may be much bigger than the that we are actually processing. + /// For example if a graph update is performed, the will just cover the tiles that are recalculated, + /// while will contain all tiles in the graph. + /// + public Int2 graphTileCount; + + /// + /// Rectangle of tiles that we are processing. + /// + /// (xmax, ymax) must be smaller than graphTileCount. + /// If for examples is (10, 10) and is {2, 3, 5, 6} then we are processing tiles (2, 3) to (5, 6) inclusive. + /// + public IntRect tileRect; + + /// Initial penalty for all nodes in the tile + public uint initialPenalty; + + /// + /// If true, all triangles will be guaranteed to be laid out in clockwise order. + /// If false, their original order will be preserved. + /// + public bool recalculateNormals; + + /// Size of a tile in world units along the graph's X and Z axes + public Vector2 tileWorldSize; + + /// Matrix to convert from graph space to world space + public Matrix4x4 graphToWorldSpace; + + public void Execute () { + var tiles = (NavmeshTile[])this.tiles.Target; + Assert.AreEqual(tileMeshes.Length, tiles.Length); + Assert.AreEqual(tileRect.Area, tileMeshes.Length); + Assert.IsTrue(tileRect.xmax < graphTileCount.x); + Assert.IsTrue(tileRect.ymax < graphTileCount.y); + + var tileRectWidth = tileRect.Width; + var tileRectDepth = tileRect.Height; + + for (int z = 0; z < tileRectDepth; z++) { + for (int x = 0; x < tileRectWidth; x++) { + var tileIndex = z*tileRectWidth + x; + // If we are just updating a part of the graph we still want to assign the nodes the proper global tile index + var graphTileIndex = (z + tileRect.ymin)*graphTileCount.x + (x + tileRect.xmin); + var mesh = tileMeshes[tileIndex]; + + // Convert tile space to graph space and world space + var verticesInGraphSpace = mesh.verticesInTileSpace.AsUnsafeSpan().Clone(Allocator.Persistent); + var verticesInWorldSpace = verticesInGraphSpace.Clone(Allocator.Persistent); + var tileSpaceToGraphSpaceOffset = (Int3) new Vector3(tileWorldSize.x * (x + tileRect.xmin), 0, tileWorldSize.y * (z + tileRect.ymin)); + for (int i = 0; i < verticesInGraphSpace.Length; i++) { + var v = verticesInGraphSpace[i] + tileSpaceToGraphSpaceOffset; + verticesInGraphSpace[i] = v; + verticesInWorldSpace[i] = (Int3)graphToWorldSpace.MultiplyPoint3x4((Vector3)v); + } + + // Create a new navmesh tile and assign its settings + var triangles = mesh.triangles.AsUnsafeSpan().Clone(Allocator.Persistent); + var tile = new NavmeshTile { + x = x + tileRect.xmin, + z = z + tileRect.ymin, + w = 1, + d = 1, + tris = triangles, + vertsInGraphSpace = verticesInGraphSpace, + verts = verticesInWorldSpace, + bbTree = new BBTree(triangles, verticesInGraphSpace), + nodes = new TriangleMeshNode[triangles.Length/3], + // Leave empty for now, it will be filled in later + graph = null, + }; + + Profiler.BeginSample("CreateNodes"); + NavmeshBase.CreateNodes(tile, tile.tris, graphTileIndex, graphIndex, mesh.tags.AsUnsafeSpan(), false, null, initialPenalty, false); + Profiler.EndSample(); + + tiles[tileIndex] = tile; + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCreateTiles.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCreateTiles.cs.meta new file mode 100644 index 0000000..3f72140 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobCreateTiles.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b86cf43938afd654a8f1b711e55977d7 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobTransformTileCoordinates.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobTransformTileCoordinates.cs new file mode 100644 index 0000000..b5b1a67 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobTransformTileCoordinates.cs @@ -0,0 +1,32 @@ +using Unity.Burst; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using UnityEngine; + +namespace Pathfinding.Graphs.Navmesh.Jobs { + /// + /// Transforms vertices from voxel coordinates to tile coordinates. + /// + /// This essentially constitutes multiplying the vertices by the . + /// + /// Note: The input space is in raw voxel coordinates, the output space is in tile coordinates stored in millimeters (as is typical for the Int3 struct. See ). + /// + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobTransformTileCoordinates : IJob { + /// Element type Int3 + public unsafe UnsafeAppendBuffer* vertices; + public Matrix4x4 matrix; + + public void Execute () { + unsafe { + int vertexCount = vertices->Length / UnsafeUtility.SizeOf(); + for (int i = 0; i < vertexCount; i++) { + // Transform from voxel indices to a proper Int3 coordinate, then convert it to a Vector3 float coordinate + var vPtr1 = (Int3*)vertices->Ptr + i; + var p = new Vector3(vPtr1->x, vPtr1->y, vPtr1->z); + *vPtr1 = (Int3)matrix.MultiplyPoint3x4(p); + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobTransformTileCoordinates.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobTransformTileCoordinates.cs.meta new file mode 100644 index 0000000..291734c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobTransformTileCoordinates.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ff97d8db3ca9a074dbfbd83fa5ad16be +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobWriteNodeConnections.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobWriteNodeConnections.cs new file mode 100644 index 0000000..ea8ef05 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobWriteNodeConnections.cs @@ -0,0 +1,60 @@ +using Pathfinding.Util; +using Unity.Collections; +using Unity.Jobs; +using UnityEngine.Assertions; +using UnityEngine.Profiling; + +namespace Pathfinding.Graphs.Navmesh.Jobs { + /// + /// Writes connections to each node in each tile. + /// + /// It also calculates the connection costs between nodes. + /// + /// This job is run after all tiles have been built and the connections have been calculated. + /// + /// See: + /// + public struct JobWriteNodeConnections : IJob { + /// Connections for each tile + [ReadOnly] + public NativeArray nodeConnections; + /// Array of + public System.Runtime.InteropServices.GCHandle tiles; + + public void Execute () { + var tiles = (NavmeshTile[])this.tiles.Target; + Assert.AreEqual(nodeConnections.Length, tiles.Length); + + for (int i = 0; i < tiles.Length; i++) { + Profiler.BeginSample("CreateConnections"); + var connections = nodeConnections[i]; + Apply(tiles[i].nodes, connections); + connections.neighbourCounts.Dispose(); + connections.neighbours.Dispose(); + Profiler.EndSample(); + } + } + + void Apply (TriangleMeshNode[] nodes, JobCalculateTriangleConnections.TileNodeConnectionsUnsafe connections) { + var neighbourCountsReader = connections.neighbourCounts.AsReader(); + var neighboursReader = connections.neighbours.AsReader(); + + for (int i = 0; i < nodes.Length; i++) { + var node = nodes[i]; + var neighbourCount = neighbourCountsReader.ReadNext(); + var conns = node.connections = ArrayPool.ClaimWithExactLength(neighbourCount); + for (int j = 0; j < neighbourCount; j++) { + var otherIndex = neighboursReader.ReadNext(); + var shapeEdgeInfo = (byte)neighboursReader.ReadNext(); + var other = nodes[otherIndex]; + var cost = (node.position - other.position).costMagnitude; + conns[j] = new Connection( + other, + (uint)cost, + shapeEdgeInfo + ); + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobWriteNodeConnections.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobWriteNodeConnections.cs.meta new file mode 100644 index 0000000..91359d9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Jobs/JobWriteNodeConnections.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: eea3ec9fc5dd8604c9902e09277d86d2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/NavmeshTile.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/NavmeshTile.cs new file mode 100644 index 0000000..fd5adc7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/NavmeshTile.cs @@ -0,0 +1,106 @@ +namespace Pathfinding.Graphs.Navmesh { + using Pathfinding.Util; + using Unity.Collections; + + /// + /// A single tile in a recast or navmesh graph. + /// + /// A tile is a single rectangular (but usually square) part of the graph. + /// Tiles can be updated individually, which is great for large worlds where updating the whole graph would take a long time. + /// + public class NavmeshTile : INavmeshHolder { + /// + /// All vertices in the tile. + /// The vertices are in graph space. + /// + /// This represents an allocation using the Persistent allocator. + /// + public UnsafeSpan vertsInGraphSpace; + /// + /// All vertices in the tile. + /// The vertices are in world space. + /// + /// This represents an allocation using the Persistent allocator. + /// + public UnsafeSpan verts; + /// + /// All triangle indices in the tile. + /// One triangle is 3 indices. + /// The triangles are in the same order as the . + /// + /// This represents an allocation using the Persistent allocator. + /// + public UnsafeSpan tris; + + /// Tile X Coordinate + public int x; + + /// Tile Z Coordinate + public int z; + + /// + /// Width, in tile coordinates. + /// Warning: Widths other than 1 are not supported. This is mainly here for possible future features. + /// + public int w; + + /// + /// Depth, in tile coordinates. + /// Warning: Depths other than 1 are not supported. This is mainly here for possible future features. + /// + public int d; + + /// All nodes in the tile + public TriangleMeshNode[] nodes; + + /// Bounding Box Tree for node lookups + public BBTree bbTree; + + /// Temporary flag used for batching + public bool flag; + + /// The graph which contains this tile + public NavmeshBase graph; + + #region INavmeshHolder implementation + + public void GetTileCoordinates (int tileIndex, out int x, out int z) { + x = this.x; + z = this.z; + } + + public int GetVertexArrayIndex (int index) { + return index & NavmeshBase.VertexIndexMask; + } + + /// Get a specific vertex in the tile + public Int3 GetVertex (int index) { + int idx = index & NavmeshBase.VertexIndexMask; + + return verts[idx]; + } + + public Int3 GetVertexInGraphSpace (int index) { + return vertsInGraphSpace[index & NavmeshBase.VertexIndexMask]; + } + + /// Transforms coordinates from graph space to world space + public GraphTransform transform { get { return graph.transform; } } + + #endregion + + public void GetNodes (System.Action action) { + if (nodes == null) return; + for (int i = 0; i < nodes.Length; i++) action(nodes[i]); + } + + public void Dispose () { + unsafe { + bbTree.Dispose(); + vertsInGraphSpace.Free(Allocator.Persistent); + verts.Free(Allocator.Persistent); + tris.Free(Allocator.Persistent); + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/NavmeshTile.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/NavmeshTile.cs.meta new file mode 100644 index 0000000..b37dca1 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/NavmeshTile.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 7408cbadf2e744d22853a92b15abede1 +timeCreated: 1474405146 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastBuilder.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastBuilder.cs new file mode 100644 index 0000000..2cd925b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastBuilder.cs @@ -0,0 +1,49 @@ +using Pathfinding.Graphs.Navmesh.Jobs; + +namespace Pathfinding.Graphs.Navmesh { + /// Helper methods for scanning a recast graph + public struct RecastBuilder { + /// + /// Builds meshes for the given tiles in a graph. + /// Call Schedule on the returned object to actually start the job. + /// + /// You may want to adjust the settings on the returned object before calling Schedule. + /// + /// + /// // Scans the first 6x6 chunk of tiles of the recast graph (the IntRect uses inclusive coordinates) + /// var graph = AstarPath.active.data.recastGraph; + /// var buildSettings = RecastBuilder.BuildTileMeshes(graph, new TileLayout(graph), new IntRect(0, 0, 5, 5)); + /// var disposeArena = new Pathfinding.Jobs.DisposeArena(); + /// var promise = buildSettings.Schedule(disposeArena); + /// + /// AstarPath.active.AddWorkItem(() => { + /// // Block until the asynchronous job completes + /// var result = promise.Complete(); + /// TileMeshes tiles = result.tileMeshes.ToManaged(); + /// // Take the scanned tiles and place them in the graph, + /// // but not at their original location, but 2 tiles away, rotated 90 degrees. + /// tiles.tileRect = tiles.tileRect.Offset(new Int2(2, 0)); + /// tiles.Rotate(1); + /// graph.ReplaceTiles(tiles); + /// + /// // Dispose unmanaged data + /// disposeArena.DisposeAll(); + /// result.Dispose(); + /// }); + /// + /// + public static TileBuilder BuildTileMeshes (RecastGraph graph, TileLayout tileLayout, IntRect tileRect) { + return new TileBuilder(graph, tileLayout, tileRect); + } + + /// + /// Builds nodes given some tile meshes. + /// Call Schedule on the returned object to actually start the job. + /// + /// See: + /// + public static JobBuildNodes BuildNodeTiles (RecastGraph graph, TileLayout tileLayout) { + return new JobBuildNodes(graph, tileLayout); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastBuilder.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastBuilder.cs.meta new file mode 100644 index 0000000..6682ef1 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastBuilder.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b6b7a26d35ca0154fa87ac69a555cce1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastMeshGatherer.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastMeshGatherer.cs new file mode 100644 index 0000000..0a0e180 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastMeshGatherer.cs @@ -0,0 +1,1134 @@ +using UnityEngine; +using System.Collections.Generic; +using Unity.Mathematics; +using Unity.Collections; +using Unity.Burst; + +namespace Pathfinding.Graphs.Navmesh { + using System; + using Pathfinding; + using Voxelization.Burst; + using Pathfinding.Util; + using Pathfinding.Jobs; + using Pathfinding.Drawing; + using UnityEngine.Profiling; + + [BurstCompile] + public class RecastMeshGatherer { + readonly int terrainDownsamplingFactor; + public readonly LayerMask mask; + public readonly List tagMask; + readonly float maxColliderApproximationError; + public readonly Bounds bounds; + public readonly UnityEngine.SceneManagement.Scene scene; + Dictionary cachedMeshes = new Dictionary(); + readonly Dictionary cachedTreePrefabs = new Dictionary(); + readonly List > vertexBuffers; + readonly List > triangleBuffers; + readonly List meshData; + readonly RecastGraph.PerLayerModification[] modificationsByLayer; + readonly RecastGraph.PerLayerModification[] modificationsByLayer2D; +#if UNITY_EDITOR + readonly List<(UnityEngine.Object, Mesh)> meshesUnreadableAtRuntime = new List<(UnityEngine.Object, Mesh)>(); +#else + bool anyNonReadableMesh = false; +#endif + + List meshes; + List dummyMaterials = new List(); + + public RecastMeshGatherer (UnityEngine.SceneManagement.Scene scene, Bounds bounds, int terrainDownsamplingFactor, LayerMask mask, List tagMask, List perLayerModifications, float maxColliderApproximationError) { + // Clamp to at least 1 since that's the resolution of the heightmap + terrainDownsamplingFactor = Math.Max(terrainDownsamplingFactor, 1); + + this.bounds = bounds; + this.terrainDownsamplingFactor = terrainDownsamplingFactor; + this.mask = mask; + this.tagMask = tagMask ?? new List(); + this.maxColliderApproximationError = maxColliderApproximationError; + this.scene = scene; + meshes = ListPool.Claim(); + vertexBuffers = ListPool >.Claim(); + triangleBuffers = ListPool >.Claim(); + cachedMeshes = ObjectPoolSimple >.Claim(); + meshData = ListPool.Claim(); + modificationsByLayer = RecastGraph.PerLayerModification.ToLayerLookup(perLayerModifications, RecastGraph.PerLayerModification.Default); + // 2D colliders default to being unwalkable + var default2D = RecastGraph.PerLayerModification.Default; + default2D.mode = RecastMeshObj.Mode.UnwalkableSurface; + modificationsByLayer2D = RecastGraph.PerLayerModification.ToLayerLookup(perLayerModifications, default2D); + } + + struct TreeInfo { + public List submeshes; + public bool supportsRotation; + } + + public struct MeshCollection : IArenaDisposable { + List > vertexBuffers; + List > triangleBuffers; + public NativeArray meshes; +#if UNITY_EDITOR + public List<(UnityEngine.Object, Mesh)> meshesUnreadableAtRuntime; +#endif + + public MeshCollection (List > vertexBuffers, List > triangleBuffers, NativeArray meshes +#if UNITY_EDITOR + , List<(UnityEngine.Object, Mesh)> meshesUnreadableAtRuntime +#endif + ) { + this.vertexBuffers = vertexBuffers; + this.triangleBuffers = triangleBuffers; + this.meshes = meshes; +#if UNITY_EDITOR + this.meshesUnreadableAtRuntime = meshesUnreadableAtRuntime; +#endif + } + + void IArenaDisposable.DisposeWith (DisposeArena arena) { + for (int i = 0; i < vertexBuffers.Count; i++) { + arena.Add(vertexBuffers[i]); + arena.Add(triangleBuffers[i]); + } + arena.Add(meshes); + } + } + + [BurstCompile] + static void CalculateBounds (ref UnsafeSpan vertices, ref float4x4 localToWorldMatrix, out Bounds bounds) { + if (vertices.Length == 0) { + bounds = new Bounds(); + } else { + float3 max = float.NegativeInfinity; + float3 min = float.PositiveInfinity; + for (uint i = 0; i < vertices.Length; i++) { + var v = math.transform(localToWorldMatrix, vertices[i]); + max = math.max(max, v); + min = math.min(min, v); + } + bounds = new Bounds((min+max)*0.5f, max-min); + } + } + + public MeshCollection Finalize () { +#if UNITY_EDITOR + // This skips the Mesh.isReadable check + Mesh.MeshDataArray data = UnityEditor.MeshUtility.AcquireReadOnlyMeshData(meshData); +#else + Mesh.MeshDataArray data = Mesh.AcquireReadOnlyMeshData(meshData); +#endif + var meshes = new NativeArray(this.meshes.Count, Allocator.Persistent); + int meshBufferOffset = vertexBuffers.Count; + + UnityEngine.Profiling.Profiler.BeginSample("Copying vertices"); + // TODO: We should be able to hold the `data` for the whole scan and not have to copy all vertices/triangles + for (int i = 0; i < data.Length; i++) { + MeshUtility.GetMeshData(data, i, out var verts, out var tris); + vertexBuffers.Add(verts); + triangleBuffers.Add(tris); + } + UnityEngine.Profiling.Profiler.EndSample(); + + UnityEngine.Profiling.Profiler.BeginSample("Creating RasterizationMeshes"); + for (int i = 0; i < meshes.Length; i++) { + var gatheredMesh = this.meshes[i]; + int bufferIndex; + if (gatheredMesh.meshDataIndex >= 0) { + bufferIndex = meshBufferOffset + gatheredMesh.meshDataIndex; + } else { + bufferIndex = -(gatheredMesh.meshDataIndex+1); + } + + var bounds = gatheredMesh.bounds; + var vertexSpan = vertexBuffers[bufferIndex].Reinterpret().AsUnsafeReadOnlySpan(); + if (bounds == new Bounds()) { + // Recalculate bounding box + float4x4 m = gatheredMesh.matrix; + CalculateBounds(ref vertexSpan, ref m, out bounds); + } + + var triangles = triangleBuffers[bufferIndex]; + meshes[i] = new RasterizationMesh { + vertices = vertexSpan, + triangles = triangles.AsUnsafeSpan().Slice(gatheredMesh.indexStart, (gatheredMesh.indexEnd != -1 ? gatheredMesh.indexEnd : triangles.Length) - gatheredMesh.indexStart), + area = gatheredMesh.area, + areaIsTag = gatheredMesh.areaIsTag, + bounds = bounds, + matrix = gatheredMesh.matrix, + solid = gatheredMesh.solid, + doubleSided = gatheredMesh.doubleSided, + flatten = gatheredMesh.flatten, + }; + } + UnityEngine.Profiling.Profiler.EndSample(); + + cachedMeshes.Clear(); + ObjectPoolSimple >.Release(ref cachedMeshes); + ListPool.Release(ref this.meshes); + + data.Dispose(); + + return new MeshCollection( + vertexBuffers, + triangleBuffers, + meshes +#if UNITY_EDITOR + , this.meshesUnreadableAtRuntime +#endif + ); + } + + /// + /// Add vertex and triangle buffers that can later be used to create a . + /// + /// The returned index can be used in the field of the struct. + /// + public int AddMeshBuffers (Vector3[] vertices, int[] triangles) { + return AddMeshBuffers(new NativeArray(vertices, Allocator.Persistent), new NativeArray(triangles, Allocator.Persistent)); + } + + /// + /// Add vertex and triangle buffers that can later be used to create a . + /// + /// The returned index can be used in the field of the struct. + /// + public int AddMeshBuffers (NativeArray vertices, NativeArray triangles) { + var meshDataIndex = -vertexBuffers.Count-1; + + vertexBuffers.Add(vertices); + triangleBuffers.Add(triangles); + return meshDataIndex; + } + + /// Add a mesh to the list of meshes to rasterize + public void AddMesh (Renderer renderer, Mesh gatheredMesh) { + if (ConvertMeshToGatheredMesh(renderer, gatheredMesh, out var gm)) { + meshes.Add(gm); + } + } + + /// Add a mesh to the list of meshes to rasterize + public void AddMesh (GatheredMesh gatheredMesh) { + meshes.Add(gatheredMesh); + } + + /// Holds info about a mesh to be rasterized + public struct GatheredMesh { + /// + /// Index in the meshData array. + /// Can be retrieved from the method. + /// + public int meshDataIndex; + /// + /// Area ID of the mesh. 0 means walkable, and -1 indicates that the mesh should be treated as unwalkable. + /// Other positive values indicate a custom area ID which will create a seam in the navmesh. + /// + public int area; + /// Start index in the triangle array + public int indexStart; + /// End index in the triangle array. -1 indicates the end of the array. + public int indexEnd; + + + /// World bounds of the mesh. Assumed to already be multiplied with the . + public Bounds bounds; + + /// Matrix to transform the vertices by + public Matrix4x4 matrix; + + /// + /// If true then the mesh will be treated as solid and its interior will be unwalkable. + /// The unwalkable region will be the minimum to maximum y coordinate in each cell. + /// + public bool solid; + /// See + public bool doubleSided; + /// See + public bool flatten; + /// See + public bool areaIsTag; + + /// + /// Recalculate the from the vertices. + /// + /// The bounds will not be recalculated immediately. + /// + public void RecalculateBounds () { + // This will cause the bounds to be recalculated later + bounds = new Bounds(); + } + + public void ApplyRecastMeshObj (RecastMeshObj recastMeshObj) { + area = AreaFromSurfaceMode(recastMeshObj.mode, recastMeshObj.surfaceID); + areaIsTag = recastMeshObj.mode == RecastMeshObj.Mode.WalkableSurfaceWithTag; + solid |= recastMeshObj.solid; + } + + public void ApplyLayerModification (RecastGraph.PerLayerModification modification) { + area = AreaFromSurfaceMode(modification.mode, modification.surfaceID); + areaIsTag = modification.mode == RecastMeshObj.Mode.WalkableSurfaceWithTag; + } + } + + enum MeshType { + Mesh, + Box, + Capsule, + } + + struct MeshCacheItem : IEquatable { + public MeshType type; + public Mesh mesh; + public int rows; + public int quantizedHeight; + + public MeshCacheItem (Mesh mesh) { + type = MeshType.Mesh; + this.mesh = mesh; + rows = 0; + quantizedHeight = 0; + } + + public static readonly MeshCacheItem Box = new MeshCacheItem { + type = MeshType.Box, + mesh = null, + rows = 0, + quantizedHeight = 0, + }; + + public bool Equals (MeshCacheItem other) { + return type == other.type && mesh == other.mesh && rows == other.rows && quantizedHeight == other.quantizedHeight; + } + + public override int GetHashCode () { + return (((int)type * 31 ^ (mesh != null ? mesh.GetHashCode() : -1)) * 31 ^ rows) * 31 ^ quantizedHeight; + } + } + + bool MeshFilterShouldBeIncluded (MeshFilter filter) { + if (filter.TryGetComponent(out var rend)) { + if (filter.sharedMesh != null && rend.enabled && (((1 << filter.gameObject.layer) & mask) != 0 || (tagMask.Count > 0 && tagMask.Contains(filter.tag)))) { + if (!(filter.TryGetComponent(out var rmo) && rmo.enabled)) { + return true; + } + } + } + return false; + } + + bool ConvertMeshToGatheredMesh (Renderer renderer, Mesh mesh, out GatheredMesh gatheredMesh) { + // Ignore meshes that do not have a Position vertex attribute. + // This can happen for meshes that are empty, i.e. have no vertices at all. + if (!mesh.HasVertexAttribute(UnityEngine.Rendering.VertexAttribute.Position)) { + gatheredMesh = default; + return false; + } + +#if !UNITY_EDITOR + if (!mesh.isReadable) { + // Cannot scan this + if (!anyNonReadableMesh) { + Debug.LogError("Some meshes could not be included when scanning the graph because they are marked as not readable. This includes the mesh '" + mesh.name + "'. You need to mark the mesh with read/write enabled in the mesh importer. Alternatively you can only rasterize colliders and not meshes. Mesh Collider meshes still need to be readable.", mesh); + } + anyNonReadableMesh = true; + gatheredMesh = default; + return false; + } +#endif + + renderer.GetSharedMaterials(dummyMaterials); + var submeshStart = renderer is MeshRenderer mrend ? mrend.subMeshStartIndex : 0; + var submeshCount = dummyMaterials.Count; + + int indexStart = 0; + int indexEnd = -1; + if (submeshStart > 0 || submeshCount < mesh.subMeshCount) { + var a = mesh.GetSubMesh(submeshStart); + var b = mesh.GetSubMesh(submeshStart + submeshCount - 1); + indexStart = a.indexStart; + indexEnd = b.indexStart + b.indexCount; + } + + // Check the cache to avoid allocating + // a new array unless necessary + if (!cachedMeshes.TryGetValue(new MeshCacheItem(mesh), out int meshBufferIndex)) { +#if UNITY_EDITOR + if (!mesh.isReadable) meshesUnreadableAtRuntime.Add((renderer, mesh)); +#endif + meshBufferIndex = meshData.Count; + meshData.Add(mesh); + cachedMeshes[new MeshCacheItem(mesh)] = meshBufferIndex; + } + + gatheredMesh = new GatheredMesh { + meshDataIndex = meshBufferIndex, + bounds = renderer.bounds, + indexStart = indexStart, + indexEnd = indexEnd, + matrix = renderer.localToWorldMatrix, + doubleSided = false, + flatten = false, + }; + return true; + } + + GatheredMesh? GetColliderMesh (MeshCollider collider, Matrix4x4 localToWorldMatrix) { + if (collider.sharedMesh != null) { + Mesh mesh = collider.sharedMesh; + + // Ignore meshes that do not have a Position vertex attribute. + // This can happen for meshes that are empty, i.e. have no vertices at all. + if (!mesh.HasVertexAttribute(UnityEngine.Rendering.VertexAttribute.Position)) { + return null; + } + +#if !UNITY_EDITOR + if (!mesh.isReadable) { + // Cannot scan this + if (!anyNonReadableMesh) { + Debug.LogError("Some mesh collider meshes could not be included when scanning the graph because they are marked as not readable. This includes the mesh '" + mesh.name + "'. You need to mark the mesh with read/write enabled in the mesh importer.", mesh); + } + anyNonReadableMesh = true; + return null; + } +#endif + + // Check the cache to avoid allocating + // a new array unless necessary + if (!cachedMeshes.TryGetValue(new MeshCacheItem(mesh), out int meshDataIndex)) { +#if UNITY_EDITOR + if (!mesh.isReadable) meshesUnreadableAtRuntime.Add((collider, mesh)); +#endif + meshDataIndex = meshData.Count; + meshData.Add(mesh); + cachedMeshes[new MeshCacheItem(mesh)] = meshDataIndex; + } + + return new GatheredMesh { + meshDataIndex = meshDataIndex, + bounds = collider.bounds, + areaIsTag = false, + area = 0, + indexStart = 0, + indexEnd = -1, + // Treat the collider as solid iff the collider is convex + solid = collider.convex, + matrix = localToWorldMatrix, + doubleSided = false, + flatten = false, + }; + } + + return null; + } + + public void CollectSceneMeshes () { + if (tagMask.Count > 0 || mask != 0) { + // This is unfortunately the fastest way to find all mesh filters.. and it is not particularly fast. + // Note: We have to sort these because the recast graph is not completely deterministic in terms of ordering of meshes. + // Different ordering can in rare cases lead to different spans being merged which can lead to different navmeshes. + var meshFilters = UnityCompatibility.FindObjectsByTypeSorted(); + bool containedStatic = false; + + for (int i = 0; i < meshFilters.Length; i++) { + MeshFilter filter = meshFilters[i]; + + if (!MeshFilterShouldBeIncluded(filter)) continue; + + // Note, guaranteed to have a renderer as MeshFilterShouldBeIncluded checks for it. + // but it can be either a MeshRenderer or a SkinnedMeshRenderer + filter.TryGetComponent(out var rend); + + if (rend.isPartOfStaticBatch) { + // Statically batched meshes cannot be used due to Unity limitations + // log a warning about this + containedStatic = true; + } else { + // Only include it if it intersects with the graph + if (rend.bounds.Intersects(bounds)) { + if (ConvertMeshToGatheredMesh(rend, filter.sharedMesh, out var gatheredMesh)) { + gatheredMesh.ApplyLayerModification(modificationsByLayer[filter.gameObject.layer]); + meshes.Add(gatheredMesh); + } + } + } + } + + if (containedStatic) { + Debug.LogWarning("Some meshes were statically batched. These meshes can not be used for navmesh calculation" + + " due to technical constraints.\nDuring runtime scripts cannot access the data of meshes which have been statically batched.\n" + + "One way to solve this problem is to use cached startup (Save & Load tab in the inspector) to only calculate the graph when the game is not playing."); + } + } + } + + static int AreaFromSurfaceMode (RecastMeshObj.Mode mode, int surfaceID) { + switch (mode) { + default: + case RecastMeshObj.Mode.UnwalkableSurface: + return -1; + case RecastMeshObj.Mode.WalkableSurface: + return 0; + case RecastMeshObj.Mode.WalkableSurfaceWithSeam: + case RecastMeshObj.Mode.WalkableSurfaceWithTag: + return surfaceID; + } + } + + /// Find all relevant RecastMeshObj components and create ExtraMeshes for them + public void CollectRecastMeshObjs () { + var buffer = ListPool.Claim(); + + // Get all recast mesh objects inside the bounds + RecastMeshObj.GetAllInBounds(buffer, bounds); + + // Create an RasterizationMesh object + // for each RecastMeshObj + for (int i = 0; i < buffer.Count; i++) { + AddRecastMeshObj(buffer[i]); + } + + ListPool.Release(ref buffer); + } + + void AddRecastMeshObj (RecastMeshObj recastMeshObj) { + if (recastMeshObj.includeInScan == RecastMeshObj.ScanInclusion.AlwaysExclude) return; + if (recastMeshObj.includeInScan == RecastMeshObj.ScanInclusion.Auto && (((mask >> recastMeshObj.gameObject.layer) & 1) == 0 && !tagMask.Contains(recastMeshObj.tag))) return; + + recastMeshObj.ResolveMeshSource(out var filter, out var collider, out var collider2D); + + if (filter != null) { + // Add based on mesh filter + Mesh mesh = filter.sharedMesh; + if (filter.TryGetComponent(out var rend) && mesh != null) { + if (ConvertMeshToGatheredMesh(rend, filter.sharedMesh, out var gatheredMesh)) { + gatheredMesh.ApplyRecastMeshObj(recastMeshObj); + meshes.Add(gatheredMesh); + } + } + } else if (collider != null) { + // Add based on collider + + if (ConvertColliderToGatheredMesh(collider) is GatheredMesh rmesh) { + rmesh.ApplyRecastMeshObj(recastMeshObj); + meshes.Add(rmesh); + } + } else if (collider2D != null) { + // 2D colliders are handled separately + } else { + if (recastMeshObj.geometrySource == RecastMeshObj.GeometrySource.Auto) { + Debug.LogError("Couldn't get geometry source for RecastMeshObject ("+recastMeshObj.gameObject.name +"). It didn't have a collider or MeshFilter+Renderer attached", recastMeshObj.gameObject); + } else { + Debug.LogError("Couldn't get geometry source for RecastMeshObject ("+recastMeshObj.gameObject.name +"). It didn't have a " + recastMeshObj.geometrySource + " attached", recastMeshObj.gameObject); + } + } + } + + public void CollectTerrainMeshes (bool rasterizeTrees, float desiredChunkSize) { + // Find all terrains in the scene + var terrains = Terrain.activeTerrains; + + if (terrains.Length > 0) { + // Loop through all terrains in the scene + for (int j = 0; j < terrains.Length; j++) { + if (terrains[j].terrainData == null) continue; + + Profiler.BeginSample("Generate terrain chunks"); + GenerateTerrainChunks(terrains[j], bounds, desiredChunkSize); + Profiler.EndSample(); + + if (rasterizeTrees) { + Profiler.BeginSample("Find tree meshes"); + // Rasterize all tree colliders on this terrain object + CollectTreeMeshes(terrains[j]); + Profiler.EndSample(); + } + } + } + } + + void GenerateTerrainChunks (Terrain terrain, Bounds bounds, float desiredChunkSize) { + var terrainData = terrain.terrainData; + + if (terrainData == null) + throw new ArgumentException("Terrain contains no terrain data"); + + Vector3 offset = terrain.GetPosition(); + Vector3 center = offset + terrainData.size * 0.5F; + + // Figure out the bounds of the terrain in world space + var terrainBounds = new Bounds(center, terrainData.size); + + // Only include terrains which intersects the graph + if (!terrainBounds.Intersects(bounds)) + return; + + // Original heightmap size + int heightmapWidth = terrainData.heightmapResolution; + int heightmapDepth = terrainData.heightmapResolution; + + // Size of a single sample + Vector3 sampleSize = terrainData.heightmapScale; + sampleSize.y = terrainData.size.y; + + // Make chunks at least 12 quads wide + // since too small chunks just decreases performance due + // to the overhead of checking for bounds and similar things + const int MinChunkSize = 12; + + // Find the number of samples along each edge that corresponds to a world size of desiredChunkSize + // Then round up to the nearest multiple of terrainSampleSize + var chunkSizeAlongX = Mathf.CeilToInt(Mathf.Max(desiredChunkSize / (sampleSize.x * terrainDownsamplingFactor), MinChunkSize)) * terrainDownsamplingFactor; + var chunkSizeAlongZ = Mathf.CeilToInt(Mathf.Max(desiredChunkSize / (sampleSize.z * terrainDownsamplingFactor), MinChunkSize)) * terrainDownsamplingFactor; + chunkSizeAlongX = Mathf.Min(chunkSizeAlongX, heightmapWidth); + chunkSizeAlongZ = Mathf.Min(chunkSizeAlongZ, heightmapDepth); + var worldChunkSizeAlongX = chunkSizeAlongX * sampleSize.x; + var worldChunkSizeAlongZ = chunkSizeAlongZ * sampleSize.z; + + // Figure out which chunks might intersect the bounding box + var allChunks = new IntRect(0, 0, heightmapWidth / chunkSizeAlongX, heightmapDepth / chunkSizeAlongZ); + var chunks = float.IsFinite(bounds.size.x) ? new IntRect( + Mathf.FloorToInt((bounds.min.x - offset.x) / worldChunkSizeAlongX), + Mathf.FloorToInt((bounds.min.z - offset.z) / worldChunkSizeAlongZ), + Mathf.FloorToInt((bounds.max.x - offset.x) / worldChunkSizeAlongX), + Mathf.FloorToInt((bounds.max.z - offset.z) / worldChunkSizeAlongZ) + ) : allChunks; + chunks = IntRect.Intersection(chunks, allChunks); + if (!chunks.IsValid()) return; + + // Sample the terrain heightmap + var sampleRect = new IntRect( + chunks.xmin * chunkSizeAlongX, + chunks.ymin * chunkSizeAlongZ, + Mathf.Min(heightmapWidth, (chunks.xmax+1) * chunkSizeAlongX) - 1, + Mathf.Min(heightmapDepth, (chunks.ymax+1) * chunkSizeAlongZ) - 1 + ); + float[, ] heights = terrainData.GetHeights( + sampleRect.xmin, + sampleRect.ymin, + sampleRect.Width, + sampleRect.Height + ); + bool[, ] holes = terrainData.GetHoles( + sampleRect.xmin, + sampleRect.ymin, + sampleRect.Width - 1, + sampleRect.Height - 1 + ); + + var chunksOffset = offset + new Vector3(chunks.xmin * chunkSizeAlongX * sampleSize.x, 0, chunks.ymin * chunkSizeAlongZ * sampleSize.z); + for (int z = chunks.ymin; z <= chunks.ymax; z++) { + for (int x = chunks.xmin; x <= chunks.xmax; x++) { + var chunk = ConvertHeightmapChunkToGatheredMesh( + heights, + holes, + sampleSize, + chunksOffset, + (x - chunks.xmin) * chunkSizeAlongX, + (z - chunks.ymin) * chunkSizeAlongZ, + chunkSizeAlongX, + chunkSizeAlongZ, + terrainDownsamplingFactor + ); + chunk.ApplyLayerModification(modificationsByLayer[terrain.gameObject.layer]); + meshes.Add(chunk); + } + } + } + + /// Returns ceil(lhs/rhs), i.e lhs/rhs rounded up + static int CeilDivision (int lhs, int rhs) { + return (lhs + rhs - 1)/rhs; + } + + /// Generates a terrain chunk mesh + public GatheredMesh ConvertHeightmapChunkToGatheredMesh (float[, ] heights, bool[,] holes, Vector3 sampleSize, Vector3 offset, int x0, int z0, int width, int depth, int stride) { + // Downsample to a smaller mesh (full resolution will take a long time to rasterize) + // Round up the width to the nearest multiple of terrainSampleSize and then add 1 + // (off by one because there are vertices at the edge of the mesh) + var heightmapDepth = heights.GetLength(0); + var heightmapWidth = heights.GetLength(1); + int resultWidth = CeilDivision(Mathf.Min(width, heightmapWidth - x0), stride) + 1; + int resultDepth = CeilDivision(Mathf.Min(depth, heightmapDepth - z0), stride) + 1; + + // Create a mesh from the heightmap + var numVerts = resultWidth * resultDepth; + var verts = new NativeArray(numVerts, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + + int numTris = (resultWidth-1)*(resultDepth-1)*2*3; + var tris = new NativeArray(numTris, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + // Using an UnsafeSpan instead of a NativeArray is much faster when writing to the array from C# + var vertsSpan = verts.AsUnsafeSpan(); + + // Create lots of vertices + for (int z = 0; z < resultDepth; z++) { + int sampleZ = Math.Min(z0 + z*stride, heightmapDepth-1); + for (int x = 0; x < resultWidth; x++) { + int sampleX = Math.Min(x0 + x*stride, heightmapWidth-1); + vertsSpan[z*resultWidth + x] = new Vector3(sampleX * sampleSize.x, heights[sampleZ, sampleX]*sampleSize.y, sampleZ * sampleSize.z) + offset; + } + } + + // Create the mesh by creating triangles in a grid like pattern + int triangleIndex = 0; + var trisSpan = tris.AsUnsafeSpan(); + for (int z = 0; z < resultDepth-1; z++) { + for (int x = 0; x < resultWidth-1; x++) { + // Try to check if the center of the cell is a hole or not. + // Note that the holes array has a size which is 1 less than the heightmap size + int sampleX = Math.Min(x0 + stride/2 + x*stride, heightmapWidth-2); + int sampleZ = Math.Min(z0 + stride/2 + z*stride, heightmapDepth-2); + + if (holes[sampleZ, sampleX]) { + // Not a hole, generate a mesh here + trisSpan[triangleIndex] = z*resultWidth + x; + trisSpan[triangleIndex+1] = (z+1)*resultWidth + x+1; + trisSpan[triangleIndex+2] = z*resultWidth + x+1; + triangleIndex += 3; + trisSpan[triangleIndex] = z*resultWidth + x; + trisSpan[triangleIndex+1] = (z+1)*resultWidth + x; + trisSpan[triangleIndex+2] = (z+1)*resultWidth + x+1; + triangleIndex += 3; + } + } + } + + + var meshDataIndex = AddMeshBuffers(verts, tris); + + var mesh = new GatheredMesh { + meshDataIndex = meshDataIndex, + // An empty bounding box indicates that it should be calculated from the vertices later. + bounds = new Bounds(), + indexStart = 0, + indexEnd = triangleIndex, + areaIsTag = false, + area = 0, + solid = false, + matrix = Matrix4x4.identity, + doubleSided = false, + flatten = false, + }; + return mesh; + } + + void CollectTreeMeshes (Terrain terrain) { + TerrainData data = terrain.terrainData; + var treeInstances = data.treeInstances; + var treePrototypes = data.treePrototypes; + + for (int i = 0; i < treeInstances.Length; i++) { + TreeInstance instance = treeInstances[i]; + TreePrototype prot = treePrototypes[instance.prototypeIndex]; + + // Make sure that the tree prefab exists + if (prot.prefab == null) { + continue; + } + + if (!cachedTreePrefabs.TryGetValue(prot.prefab, out TreeInfo treeInfo)) { + treeInfo.submeshes = new List(); + + // The unity terrain system only supports rotation for trees with a LODGroup on the root object. + // Unity still sets the instance.rotation field to values even they are not used, so we need to explicitly check for this. + treeInfo.supportsRotation = prot.prefab.TryGetComponent(out var dummy); + + var colliders = ListPool.Claim(); + var rootMatrixInv = prot.prefab.transform.localToWorldMatrix.inverse; + prot.prefab.GetComponentsInChildren(false, colliders); + for (int j = 0; j < colliders.Count; j++) { + // The prefab has a collider, use that instead + var collider = colliders[j]; + + // Generate a mesh from the collider + if (ConvertColliderToGatheredMesh(collider, rootMatrixInv * collider.transform.localToWorldMatrix) is GatheredMesh mesh) { + // For trees, we only suppport generating a mesh from a collider. So we ignore the recastMeshObj.geometrySource field. + if (collider.gameObject.TryGetComponent(out var recastMeshObj) && recastMeshObj.enabled) { + if (recastMeshObj.includeInScan == RecastMeshObj.ScanInclusion.AlwaysExclude) continue; + + mesh.ApplyRecastMeshObj(recastMeshObj); + } else { + mesh.ApplyLayerModification(modificationsByLayer[collider.gameObject.layer]); + } + + // The bounds are incorrectly based on collider.bounds. + // It is incorrect because the collider is on the prefab, not on the tree instance + // so we need to recalculate the bounds based on the actual vertex positions + mesh.RecalculateBounds(); + //mesh.matrix = collider.transform.localToWorldMatrix.inverse * mesh.matrix; + treeInfo.submeshes.Add(mesh); + } + } + + ListPool.Release(ref colliders); + cachedTreePrefabs[prot.prefab] = treeInfo; + } + + var treePosition = terrain.transform.position + Vector3.Scale(instance.position, data.size); + var instanceSize = new Vector3(instance.widthScale, instance.heightScale, instance.widthScale); + var prefabScale = Vector3.Scale(instanceSize, prot.prefab.transform.localScale); + var rotation = treeInfo.supportsRotation ? instance.rotation : 0; + var matrix = Matrix4x4.TRS(treePosition, Quaternion.AngleAxis(rotation * Mathf.Rad2Deg, Vector3.up), prefabScale); + + for (int j = 0; j < treeInfo.submeshes.Count; j++) { + var item = treeInfo.submeshes[j]; + item.matrix = matrix * item.matrix; + meshes.Add(item); + } + } + } + + bool ShouldIncludeCollider (Collider collider) { + if (!collider.enabled || collider.isTrigger || !collider.bounds.Intersects(bounds) || (collider.TryGetComponent(out var rmo) && rmo.enabled)) return false; + + var go = collider.gameObject; + if (((mask >> go.layer) & 1) != 0) return true; + + // Iterate over the tag mask and use CompareTag instead of tagMask.Includes(collider.tag), as this will not allocate. + for (int i = 0; i < tagMask.Count; i++) { + if (go.CompareTag(tagMask[i])) return true; + } + return false; + } + + public void CollectColliderMeshes () { + if (tagMask.Count == 0 && mask == 0) return; + + var physicsScene = scene.GetPhysicsScene(); + // Find all colliders that could possibly be inside the bounds + // TODO: Benchmark? + // Repeatedly do a OverlapBox check and make the buffer larger if it's too small. + int numColliders = 256; + Collider[] colliderBuffer = null; + bool finiteBounds = math.all(math.isfinite(bounds.extents)); + if (!finiteBounds) { + colliderBuffer = UnityCompatibility.FindObjectsByTypeSorted(); + numColliders = colliderBuffer.Length; + } else { + do { + if (colliderBuffer != null) ArrayPool.Release(ref colliderBuffer); + colliderBuffer = ArrayPool.Claim(numColliders * 4); + numColliders = physicsScene.OverlapBox(bounds.center, bounds.extents, colliderBuffer, Quaternion.identity, ~0, QueryTriggerInteraction.Ignore); + } while (numColliders == colliderBuffer.Length); + } + + + for (int i = 0; i < numColliders; i++) { + Collider collider = colliderBuffer[i]; + + if (ShouldIncludeCollider(collider)) { + if (ConvertColliderToGatheredMesh(collider) is GatheredMesh mesh) { + mesh.ApplyLayerModification(modificationsByLayer[collider.gameObject.layer]); + meshes.Add(mesh); + } + } + } + + if (finiteBounds) ArrayPool.Release(ref colliderBuffer); + } + + /// + /// Box Collider triangle indices can be reused for multiple instances. + /// Warning: This array should never be changed + /// + private readonly static int[] BoxColliderTris = { + 0, 1, 2, + 0, 2, 3, + + 6, 5, 4, + 7, 6, 4, + + 0, 5, 1, + 0, 4, 5, + + 1, 6, 2, + 1, 5, 6, + + 2, 7, 3, + 2, 6, 7, + + 3, 4, 0, + 3, 7, 4 + }; + + /// + /// Box Collider vertices can be reused for multiple instances. + /// Warning: This array should never be changed + /// + private readonly static Vector3[] BoxColliderVerts = { + new Vector3(-1, -1, -1), + new Vector3(1, -1, -1), + new Vector3(1, -1, 1), + new Vector3(-1, -1, 1), + + new Vector3(-1, 1, -1), + new Vector3(1, 1, -1), + new Vector3(1, 1, 1), + new Vector3(-1, 1, 1), + }; + + /// + /// Rasterizes a collider to a mesh. + /// This will pass the col.transform.localToWorldMatrix to the other overload of this function. + /// + GatheredMesh? ConvertColliderToGatheredMesh (Collider col) { + return ConvertColliderToGatheredMesh(col, col.transform.localToWorldMatrix); + } + + /// + /// Rasterizes a collider to a mesh assuming it's vertices should be multiplied with the matrix. + /// Note that the bounds of the returned RasterizationMesh is based on collider.bounds. So you might want to + /// call myExtraMesh.RecalculateBounds on the returned mesh to recalculate it if the collider.bounds would + /// not give the correct value. + /// + public GatheredMesh? ConvertColliderToGatheredMesh (Collider col, Matrix4x4 localToWorldMatrix) { + if (col is BoxCollider box) { + return RasterizeBoxCollider(box, localToWorldMatrix); + } else if (col is SphereCollider || col is CapsuleCollider) { + var scollider = col as SphereCollider; + var ccollider = col as CapsuleCollider; + + float radius = scollider != null ? scollider.radius : ccollider.radius; + float height = scollider != null ? 0 : (ccollider.height*0.5f/radius) - 1; + Quaternion rot = Quaternion.identity; + // Capsule colliders can be aligned along the X, Y or Z axis + if (ccollider != null) rot = Quaternion.Euler(ccollider.direction == 2 ? 90 : 0, 0, ccollider.direction == 0 ? 90 : 0); + Matrix4x4 matrix = Matrix4x4.TRS(scollider != null ? scollider.center : ccollider.center, rot, Vector3.one*radius); + + matrix = localToWorldMatrix * matrix; + + return RasterizeCapsuleCollider(radius, height, col.bounds, matrix); + } else if (col is MeshCollider collider) { + return GetColliderMesh(collider, localToWorldMatrix); + } + + return null; + } + + GatheredMesh RasterizeBoxCollider (BoxCollider collider, Matrix4x4 localToWorldMatrix) { + Matrix4x4 matrix = Matrix4x4.TRS(collider.center, Quaternion.identity, collider.size*0.5f); + + matrix = localToWorldMatrix * matrix; + + if (!cachedMeshes.TryGetValue(MeshCacheItem.Box, out int meshDataIndex)) { + meshDataIndex = AddMeshBuffers(BoxColliderVerts, BoxColliderTris); + cachedMeshes[MeshCacheItem.Box] = meshDataIndex; + } + + return new GatheredMesh { + meshDataIndex = meshDataIndex, + bounds = collider.bounds, + indexStart = 0, + indexEnd = -1, + areaIsTag = false, + area = 0, + solid = true, + matrix = matrix, + doubleSided = false, + flatten = false, + }; + } + + static int CircleSteps (Matrix4x4 matrix, float radius, float maxError) { + // Take the maximum scale factor among the 3 axes. + // If the current matrix has a uniform scale then they are all the same. + var maxScaleFactor = math.sqrt(math.max(math.max(math.lengthsq((Vector3)matrix.GetColumn(0)), math.lengthsq((Vector3)matrix.GetColumn(1))), math.lengthsq((Vector3)matrix.GetColumn(2)))); + var realWorldRadius = radius * maxScaleFactor; + + var cosAngle = 1 - maxError / realWorldRadius; + int steps = cosAngle < 0 ? 3 : (int)math.ceil(math.PI / math.acos(cosAngle)); + return steps; + } + + /// + /// If a circle is approximated by fewer segments, it will be slightly smaller than the original circle. + /// This factor is used to adjust the radius of the circle so that the resulting circle will have roughly the same area as the original circle. + /// + static float CircleRadiusAdjustmentFactor (int steps) { + return 0.5f * (1 - math.cos(2 * math.PI / steps)); + } + + GatheredMesh RasterizeCapsuleCollider (float radius, float height, Bounds bounds, Matrix4x4 localToWorldMatrix) { + // Calculate the number of rows to use + int rows = CircleSteps(localToWorldMatrix, radius, maxColliderApproximationError); + + int cols = rows; + + var cacheItem = new MeshCacheItem { + type = MeshType.Capsule, + mesh = null, + rows = rows, + // Capsules that differ by a very small amount in height will be rasterized in the same way + quantizedHeight = Mathf.RoundToInt(height/maxColliderApproximationError), + }; + + if (!cachedMeshes.TryGetValue(cacheItem, out var meshDataIndex)) { + // Generate a sphere/capsule mesh + + var verts = new NativeArray(rows*cols + 2, Allocator.Persistent); + + var tris = new NativeArray(rows*cols*2*3, Allocator.Persistent); + + for (int r = 0; r < rows; r++) { + for (int c = 0; c < cols; c++) { + verts[c + r*cols] = new Vector3(Mathf.Cos(c*Mathf.PI*2/cols)*Mathf.Sin((r*Mathf.PI/(rows-1))), Mathf.Cos((r*Mathf.PI/(rows-1))) + (r < rows/2 ? height : -height), Mathf.Sin(c*Mathf.PI*2/cols)*Mathf.Sin((r*Mathf.PI/(rows-1)))); + } + } + + verts[verts.Length-1] = Vector3.up; + verts[verts.Length-2] = Vector3.down; + + int triIndex = 0; + + for (int i = 0, j = cols-1; i < cols; j = i++) { + tris[triIndex + 0] = (verts.Length-1); + tris[triIndex + 1] = (0*cols + j); + tris[triIndex + 2] = (0*cols + i); + triIndex += 3; + } + + for (int r = 1; r < rows; r++) { + for (int i = 0, j = cols-1; i < cols; j = i++) { + tris[triIndex + 0] = (r*cols + i); + tris[triIndex + 1] = (r*cols + j); + tris[triIndex + 2] = ((r-1)*cols + i); + triIndex += 3; + + tris[triIndex + 0] = ((r-1)*cols + j); + tris[triIndex + 1] = ((r-1)*cols + i); + tris[triIndex + 2] = (r*cols + j); + triIndex += 3; + } + } + + for (int i = 0, j = cols-1; i < cols; j = i++) { + tris[triIndex + 0] = (verts.Length-2); + tris[triIndex + 1] = ((rows-1)*cols + j); + tris[triIndex + 2] = ((rows-1)*cols + i); + triIndex += 3; + } + + UnityEngine.Assertions.Assert.AreEqual(triIndex, tris.Length); + + // TOOD: Avoid allocating original C# array + // Store custom vertex buffers as negative indices + meshDataIndex = AddMeshBuffers(verts, tris); + cachedMeshes[cacheItem] = meshDataIndex; + } + + return new GatheredMesh { + meshDataIndex = meshDataIndex, + bounds = bounds, + areaIsTag = false, + area = 0, + indexStart = 0, + indexEnd = -1, + solid = true, + matrix = localToWorldMatrix, + doubleSided = false, + flatten = false, + }; + } + + bool ShouldIncludeCollider2D (Collider2D collider) { + // Note: Some things are already checked, namely that: + // - collider.enabled is true + // - that the bounds intersect (at least approxmately) + // - that the collider is not a trigger + + // This is not completely analogous to ShouldIncludeCollider, as this one will + // always include the collider if it has an attached RecastMeshObj, while + // 3D colliders handle RecastMeshObj components separately. + if (((mask >> collider.gameObject.layer) & 1) != 0) return true; + if ((collider.attachedRigidbody as Component ?? collider).TryGetComponent(out var rmo) && rmo.enabled && rmo.includeInScan == RecastMeshObj.ScanInclusion.AlwaysInclude) return true; + + for (int i = 0; i < tagMask.Count; i++) { + if (collider.CompareTag(tagMask[i])) return true; + } + return false; + } + + public void Collect2DColliderMeshes () { + if (tagMask.Count == 0 && mask == 0) return; + + var physicsScene = scene.GetPhysicsScene2D(); + // Find all colliders that could possibly be inside the bounds + // TODO: Benchmark? + int numColliders = 256; + Collider2D[] colliderBuffer = null; + bool finiteBounds = math.isfinite(bounds.extents.x) && math.isfinite(bounds.extents.y); + + if (!finiteBounds) { + colliderBuffer = UnityCompatibility.FindObjectsByTypeSorted(); + numColliders = colliderBuffer.Length; + } else { + // Repeatedly do a OverlapArea check and make the buffer larger if it's too small. + var min2D = (Vector2)bounds.min; + var max2D = (Vector2)bounds.max; + var filter = new ContactFilter2D().NoFilter(); + // It would be nice to add the layer mask filter here as well, + // but we cannot since a collider may have a RecastMeshObj component + // attached, and in that case we want to include it even if it is on an excluded layer. + // The user may also want to include objects based on tags. + // But we can at least exclude all triggers. + filter.useTriggers = false; + + do { + if (colliderBuffer != null) ArrayPool.Release(ref colliderBuffer); + colliderBuffer = ArrayPool.Claim(numColliders * 4); + numColliders = physicsScene.OverlapArea(min2D, max2D, filter, colliderBuffer); + } while (numColliders == colliderBuffer.Length); + } + + // Filter out colliders that should not be included + for (int i = 0; i < numColliders; i++) { + if (!ShouldIncludeCollider2D(colliderBuffer[i])) colliderBuffer[i] = null; + } + + int shapeMeshCount = ColliderMeshBuilder2D.GenerateMeshesFromColliders(colliderBuffer, numColliders, maxColliderApproximationError, out var vertices, out var indices, out var shapeMeshes); + var bufferIndex = AddMeshBuffers(vertices.Reinterpret(), indices); + + for (int i = 0; i < shapeMeshCount; i++) { + var shape = shapeMeshes[i]; + + // Skip if the shape is not inside the bounds. + // This is a more granular check than the one done by the OverlapArea call above, + // since each collider may generate multiple shapes with different bounds. + // This is particularly important for TilemapColliders which may generate a lot of shapes. + if (!bounds.Intersects(shape.bounds)) continue; + + var coll = colliderBuffer[shape.tag]; + (coll.attachedRigidbody as Component ?? coll).TryGetComponent(out var recastMeshObj); + + var rmesh = new GatheredMesh { + meshDataIndex = bufferIndex, + bounds = shape.bounds, + indexStart = shape.startIndex, + indexEnd = shape.endIndex, + areaIsTag = false, + // Colliders default to being unwalkable + area = -1, + solid = false, + matrix = shape.matrix, + doubleSided = true, + flatten = true, + }; + + if (recastMeshObj != null) { + if (recastMeshObj.includeInScan == RecastMeshObj.ScanInclusion.AlwaysExclude) continue; + rmesh.ApplyRecastMeshObj(recastMeshObj); + } else { + rmesh.ApplyLayerModification(modificationsByLayer2D[coll.gameObject.layer]); + } + + // 2D colliders are never solid + rmesh.solid = false; + + meshes.Add(rmesh); + } + + if (finiteBounds) ArrayPool.Release(ref colliderBuffer); + shapeMeshes.Dispose(); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastMeshGatherer.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastMeshGatherer.cs.meta new file mode 100644 index 0000000..da5dcf1 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/RecastMeshGatherer.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b37acf4e486d51b8394c1d8e2b0c59c2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileBuilder.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileBuilder.cs new file mode 100644 index 0000000..f68f8a4 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileBuilder.cs @@ -0,0 +1,366 @@ +using System.Collections.Generic; +using Pathfinding.Graphs.Navmesh.Jobs; +using Pathfinding.Jobs; +using Pathfinding.Util; +using Pathfinding.Graphs.Navmesh.Voxelization.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using UnityEngine; +using UnityEngine.Profiling; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Navmesh { + /// + /// Settings for building tile meshes in a recast graph. + /// + /// See: for more documentation on the individual fields. + /// See: + /// + public struct TileBuilder { + public float walkableClimb; + public RecastGraph.CollectionSettings collectionSettings; + public RecastGraph.RelevantGraphSurfaceMode relevantGraphSurfaceMode; + public RecastGraph.DimensionMode dimensionMode; + public RecastGraph.BackgroundTraversability backgroundTraversability; + + // TODO: Don't store in struct + public int tileBorderSizeInVoxels; + public float walkableHeight; + public float maxSlope; + // TODO: Specify in world units + public int characterRadiusInVoxels; + public int minRegionSize; + public float maxEdgeLength; + public float contourMaxError; + public UnityEngine.SceneManagement.Scene scene; + public TileLayout tileLayout; + public IntRect tileRect; + public List perLayerModifications; + + public class TileBuilderOutput : IProgress, System.IDisposable { + public NativeReference currentTileCounter; + public TileMeshesUnsafe tileMeshes; +#if UNITY_EDITOR + public List<(UnityEngine.Object, Mesh)> meshesUnreadableAtRuntime; +#endif + + public float Progress { + get { + var tileCount = tileMeshes.tileRect.Area; + var currentTile = Mathf.Min(tileCount, currentTileCounter.Value); + return tileCount > 0 ? currentTile / (float)tileCount : 0; // "Scanning tiles: " + currentTile + " of " + (tileCount) + " tiles..."); + } + } + + public void Dispose () { + tileMeshes.Dispose(); + if (currentTileCounter.IsCreated) currentTileCounter.Dispose(); +#if UNITY_EDITOR + if (meshesUnreadableAtRuntime != null) ListPool<(UnityEngine.Object, Mesh)>.Release(ref meshesUnreadableAtRuntime); +#endif + } + } + + public TileBuilder (RecastGraph graph, TileLayout tileLayout, IntRect tileRect) { + this.tileLayout = tileLayout; + this.tileRect = tileRect; + // A walkableClimb higher than walkableHeight can cause issues when generating the navmesh since then it can in some cases + // Both be valid for a character to walk under an obstacle and climb up on top of it (and that cannot be handled with navmesh without links) + // The editor scripts also enforce this, but we enforce it here too just to be sure + this.walkableClimb = Mathf.Min(graph.walkableClimb, graph.walkableHeight); + this.collectionSettings = graph.collectionSettings; + this.dimensionMode = graph.dimensionMode; + this.backgroundTraversability = graph.backgroundTraversability; + this.tileBorderSizeInVoxels = graph.TileBorderSizeInVoxels; + this.walkableHeight = graph.walkableHeight; + this.maxSlope = graph.maxSlope; + this.characterRadiusInVoxels = graph.CharacterRadiusInVoxels; + this.minRegionSize = Mathf.RoundToInt(graph.minRegionSize); + this.maxEdgeLength = graph.maxEdgeLength; + this.contourMaxError = graph.contourMaxError; + this.relevantGraphSurfaceMode = graph.relevantGraphSurfaceMode; + this.scene = graph.active.gameObject.scene; + this.perLayerModifications = graph.perLayerModifications; + } + + /// + /// Number of extra voxels on each side of a tile to ensure accurate navmeshes near the tile border. + /// The width of a tile is expanded by 2 times this value (1x to the left and 1x to the right) + /// + int TileBorderSizeInVoxels { + get { + return characterRadiusInVoxels + 3; + } + } + + float TileBorderSizeInWorldUnits { + get { + return TileBorderSizeInVoxels*tileLayout.cellSize; + } + } + + /// Get the world space bounds for all tiles, including an optional (graph space) padding around the tiles in the x and z axis + public Bounds GetWorldSpaceBounds (float xzPadding = 0) { + var graphSpaceBounds = tileLayout.GetTileBoundsInGraphSpace(tileRect.xmin, tileRect.ymin, tileRect.Width, tileRect.Height); + graphSpaceBounds.Expand(new Vector3(2*xzPadding, 0, 2*xzPadding)); + return tileLayout.transform.Transform(graphSpaceBounds); + } + + public RecastMeshGatherer.MeshCollection CollectMeshes (Bounds bounds) { + Profiler.BeginSample("Find Meshes for rasterization"); + var mask = collectionSettings.layerMask; + var tagMask = collectionSettings.tagMask; + if (collectionSettings.collectionMode == RecastGraph.CollectionSettings.FilterMode.Layers) { + tagMask = null; + } else { + mask = -1; + } + var meshGatherer = new RecastMeshGatherer(scene, bounds, collectionSettings.terrainHeightmapDownsamplingFactor, collectionSettings.layerMask, collectionSettings.tagMask, perLayerModifications, tileLayout.cellSize / collectionSettings.colliderRasterizeDetail); + + if (collectionSettings.rasterizeMeshes && dimensionMode == RecastGraph.DimensionMode.Dimension3D) { + Profiler.BeginSample("Find meshes"); + meshGatherer.CollectSceneMeshes(); + Profiler.EndSample(); + } + + Profiler.BeginSample("Find RecastMeshObj components"); + meshGatherer.CollectRecastMeshObjs(); + Profiler.EndSample(); + + if (collectionSettings.rasterizeTerrain && dimensionMode == RecastGraph.DimensionMode.Dimension3D) { + Profiler.BeginSample("Find terrains"); + // Split terrains up into meshes approximately the size of a single chunk + var desiredTerrainChunkSize = tileLayout.cellSize*math.max(tileLayout.tileSizeInVoxels.x, tileLayout.tileSizeInVoxels.y); + meshGatherer.CollectTerrainMeshes(collectionSettings.rasterizeTrees, desiredTerrainChunkSize); + Profiler.EndSample(); + } + + if (collectionSettings.rasterizeColliders || dimensionMode == RecastGraph.DimensionMode.Dimension2D) { + Profiler.BeginSample("Find colliders"); + if (dimensionMode == RecastGraph.DimensionMode.Dimension3D) { + meshGatherer.CollectColliderMeshes(); + } else { + meshGatherer.Collect2DColliderMeshes(); + } + Profiler.EndSample(); + } + + if (collectionSettings.onCollectMeshes != null) { + Profiler.BeginSample("Custom mesh collection"); + collectionSettings.onCollectMeshes(meshGatherer); + Profiler.EndSample(); + } + + Profiler.BeginSample("Finalizing"); + var result = meshGatherer.Finalize(); + Profiler.EndSample(); + + // Warn if no meshes were found, but only if the tile rect covers the whole graph. + // If it's just a partial update, the user is probably not interested in this warning, + // as it is completely normal that there are some empty tiles. + if (tileRect == new IntRect(0, 0, tileLayout.tileCount.x - 1, tileLayout.tileCount.y - 1) && result.meshes.Length == 0) { + Debug.LogWarning("No rasterizable objects were found contained in the layers specified by the 'mask' variables"); + } + + Profiler.EndSample(); + return result; + } + + /// A mapping from tiles to the meshes that each tile touches + public struct BucketMapping { + /// All meshes that should be voxelized + public NativeArray meshes; + /// Indices into the array + public NativeArray pointers; + /// + /// For each tile, the range of pointers in that correspond to that tile. + /// This is a cumulative sum of the number of pointers in each bucket. + /// + /// Bucket i will contain pointers in the range [i > 0 ? bucketRanges[i-1] : 0, bucketRanges[i]). + /// + /// The length is the same as the number of tiles. + /// + public NativeArray bucketRanges; + } + + /// Creates a list for every tile and adds every mesh that touches a tile to the corresponding list + BucketMapping PutMeshesIntoTileBuckets (RecastMeshGatherer.MeshCollection meshCollection, IntRect tileBuckets) { + var bucketCount = tileBuckets.Width*tileBuckets.Height; + var buckets = new NativeList[bucketCount]; + var borderExpansion = TileBorderSizeInWorldUnits; + + for (int i = 0; i < buckets.Length; i++) { + buckets[i] = new NativeList(Allocator.Persistent); + } + + var offset = -tileBuckets.Min; + var clamp = new IntRect(0, 0, tileBuckets.Width - 1, tileBuckets.Height - 1); + var meshes = meshCollection.meshes; + for (int i = 0; i < meshes.Length; i++) { + var mesh = meshes[i]; + var bounds = mesh.bounds; + var rect = tileLayout.GetTouchingTiles(bounds, borderExpansion); + rect = IntRect.Intersection(rect.Offset(offset), clamp); + for (int z = rect.ymin; z <= rect.ymax; z++) { + for (int x = rect.xmin; x <= rect.xmax; x++) { + buckets[x + z*tileBuckets.Width].Add(i); + } + } + } + + // Concat buckets + int allPointersCount = 0; + for (int i = 0; i < buckets.Length; i++) allPointersCount += buckets[i].Length; + var allPointers = new NativeArray(allPointersCount, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + var bucketRanges = new NativeArray(bucketCount, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + allPointersCount = 0; + for (int i = 0; i < buckets.Length; i++) { + // If we have an empty bucket at the end of the array then allPointersCount might be equal to allPointers.Length which would cause an assert to trigger. + // So for empty buckets don't call the copy method + if (buckets[i].Length > 0) { + NativeArray.Copy(buckets[i].AsArray(), 0, allPointers, allPointersCount, buckets[i].Length); + } + allPointersCount += buckets[i].Length; + bucketRanges[i] = allPointersCount; + buckets[i].Dispose(); + } + + return new BucketMapping { + meshes = meshCollection.meshes, + pointers = allPointers, + bucketRanges = bucketRanges, + }; + } + + public Promise Schedule (DisposeArena arena) { + var tileCount = tileRect.Area; + Assert.IsTrue(tileCount > 0); + + var tileRectWidth = tileRect.Width; + var tileRectDepth = tileRect.Height; + + // Find all meshes that could affect the graph + var worldBounds = GetWorldSpaceBounds(TileBorderSizeInWorldUnits); + if (dimensionMode == RecastGraph.DimensionMode.Dimension2D) { + // In 2D mode, the bounding box of the graph only bounds it in the X and Y dimensions + worldBounds.extents = new Vector3(worldBounds.extents.x, worldBounds.extents.y, float.PositiveInfinity); + } + var meshes = CollectMeshes(worldBounds); + + Profiler.BeginSample("PutMeshesIntoTileBuckets"); + var buckets = PutMeshesIntoTileBuckets(meshes, tileRect); + Profiler.EndSample(); + + Profiler.BeginSample("Allocating tiles"); + var tileMeshes = new NativeArray(tileCount, Allocator.Persistent, NativeArrayOptions.ClearMemory); + + int width = tileLayout.tileSizeInVoxels.x + tileBorderSizeInVoxels*2; + int depth = tileLayout.tileSizeInVoxels.y + tileBorderSizeInVoxels*2; + var cellHeight = tileLayout.CellHeight; + // TODO: Move inside BuildTileMeshBurst + var voxelWalkableHeight = (uint)(walkableHeight/cellHeight); + var voxelWalkableClimb = Mathf.RoundToInt(walkableClimb/cellHeight); + + var tileGraphSpaceBounds = new NativeArray(tileCount, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + + for (int z = 0; z < tileRectDepth; z++) { + for (int x = 0; x < tileRectWidth; x++) { + int tileIndex = x + z*tileRectWidth; + var tileBounds = tileLayout.GetTileBoundsInGraphSpace(tileRect.xmin + x, tileRect.ymin + z); + // Expand borderSize voxels on each side + tileBounds.Expand(new Vector3(1, 0, 1)*TileBorderSizeInWorldUnits*2); + tileGraphSpaceBounds[tileIndex] = tileBounds; + } + } + + Profiler.EndSample(); + Profiler.BeginSample("Scheduling jobs"); + + var builders = new TileBuilderBurst[Mathf.Max(1, Mathf.Min(tileCount, Unity.Jobs.LowLevel.Unsafe.JobsUtility.JobWorkerCount + 1))]; + var currentTileCounter = new NativeReference(0, Allocator.Persistent); + JobHandle dependencies = default; + + var relevantGraphSurfaces = new NativeList(Allocator.Persistent); + var c = RelevantGraphSurface.Root; + while (c != null) { + relevantGraphSurfaces.Add(new JobBuildRegions.RelevantGraphSurfaceInfo { + position = c.transform.position, + range = c.maxRange, + }); + c = c.Next; + } + + + // Having a few long running jobs is bad because Unity cannot inject more high priority jobs + // in between tile calculations. So we run each builder a number of times. + // Each step will just calculate one tile. + int tilesPerJob = Mathf.CeilToInt(Mathf.Sqrt(tileCount)); + // Number of tiles calculated if every builder runs once + int tilesPerStep = tilesPerJob * builders.Length; + // Round up to make sure we run the jobs enough times + // We multiply by 2 to run a bit more jobs than strictly necessary. + // This is to ensure that if one builder just gets a bunch of long running jobs + // then the other builders can steal some work from it. + int jobSteps = 2 * (tileCount + tilesPerStep - 1) / tilesPerStep; + var jobTemplate = new JobBuildTileMeshFromVoxels { + tileBuilder = builders[0], + inputMeshes = buckets, + tileGraphSpaceBounds = tileGraphSpaceBounds, + voxelWalkableClimb = voxelWalkableClimb, + voxelWalkableHeight = voxelWalkableHeight, + voxelToTileSpace = Matrix4x4.Scale(new Vector3(tileLayout.cellSize, cellHeight, tileLayout.cellSize)) * Matrix4x4.Translate(-new Vector3(1, 0, 1)*TileBorderSizeInVoxels), + cellSize = tileLayout.cellSize, + cellHeight = cellHeight, + maxSlope = Mathf.Max(maxSlope, 0.0001f), // Ensure maxSlope is not 0, as then horizontal surfaces can sometimes get excluded due to floating point errors + dimensionMode = dimensionMode, + backgroundTraversability = backgroundTraversability, + graphToWorldSpace = tileLayout.transform.matrix, + // Crop all tiles to ensure they are inside the graph bounds (even if the tiles did not line up perfectly with the bounding box). + // Add the character radius, since it will be eroded away anyway, but subtract 1 voxel to ensure the nodes are strictly inside the bounding box + graphSpaceLimits = new Vector2(tileLayout.graphSpaceSize.x + (characterRadiusInVoxels-1)*tileLayout.cellSize, tileLayout.graphSpaceSize.z + (characterRadiusInVoxels-1)*tileLayout.cellSize), + characterRadiusInVoxels = characterRadiusInVoxels, + tileBorderSizeInVoxels = tileBorderSizeInVoxels, + minRegionSize = minRegionSize, + maxEdgeLength = maxEdgeLength, + contourMaxError = contourMaxError, + maxTiles = tilesPerJob, + relevantGraphSurfaces = relevantGraphSurfaces.AsArray(), + relevantGraphSurfaceMode = this.relevantGraphSurfaceMode, + }; + jobTemplate.SetOutputMeshes(tileMeshes); + jobTemplate.SetCounter(currentTileCounter); + int maximumVoxelYCoord = (int)(tileLayout.graphSpaceSize.y / cellHeight); + for (int i = 0; i < builders.Length; i++) { + jobTemplate.tileBuilder = builders[i] = new TileBuilderBurst(width, depth, (int)voxelWalkableHeight, maximumVoxelYCoord); + var dep = new JobHandle(); + for (int j = 0; j < jobSteps; j++) { + dep = jobTemplate.Schedule(dep); + } + dependencies = JobHandle.CombineDependencies(dependencies, dep); + } + JobHandle.ScheduleBatchedJobs(); + + Profiler.EndSample(); + + arena.Add(tileGraphSpaceBounds); + arena.Add(relevantGraphSurfaces); + arena.Add(buckets.bucketRanges); + arena.Add(buckets.pointers); + // Note: buckets.meshes references data in #meshes, so we don't have to dispose it separately + arena.Add(meshes); + + // Dispose the mesh data after all jobs are completed. + // Note that the jobs use pointers to this data which are not tracked by the safety system. + for (int i = 0; i < builders.Length; i++) arena.Add(builders[i]); + + return new Promise(dependencies, new TileBuilderOutput { + tileMeshes = new TileMeshesUnsafe(tileMeshes, tileRect, new Vector2(tileLayout.TileWorldSizeX, tileLayout.TileWorldSizeZ)), + currentTileCounter = currentTileCounter, +#if UNITY_EDITOR + meshesUnreadableAtRuntime = meshes.meshesUnreadableAtRuntime, +#endif + }); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileBuilder.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileBuilder.cs.meta new file mode 100644 index 0000000..90f7f56 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileBuilder.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 1bfefed1cddc88f449cc850ad00f2f77 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileHandler.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileHandler.cs new file mode 100644 index 0000000..c182b03 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileHandler.cs @@ -0,0 +1,1258 @@ +using System; +using System.Collections.Generic; +using UnityEngine; +using Pathfinding.ClipperLib; +using UnityEngine.Profiling; + +namespace Pathfinding.Graphs.Navmesh { + using Pathfinding; + using Pathfinding.Util; + using Pathfinding.Poly2Tri; + using Unity.Collections; + using Unity.Collections.LowLevel.Unsafe; + using Unity.Mathematics; + using Pathfinding.Graphs.Util; + + /// + /// Utility class for updating tiles of navmesh/recast graphs. + /// + /// Most operations that this class does are asynchronous. + /// They will be added as work items to the AstarPath class + /// and executed when the pathfinding threads have finished + /// calculating their current paths. + /// + /// See: navmeshcutting (view in online documentation for working links) + /// See: + /// + public class TileHandler { + /// The underlaying graph which is handled by this instance + public readonly NavmeshBase graph; + + /// Number of tiles along the x axis + int tileXCount; + + /// Number of tiles along the z axis + int tileZCount; + + /// Handles polygon clipping operations + readonly Clipper clipper = new Clipper(); + + /// Cached dictionary to avoid excessive allocations + readonly Dictionary cached_Int2_int_dict = new Dictionary(); + + /// + /// Which tile type is active on each tile index. + /// This array will be tileXCount*tileZCount elements long. + /// + TileType[] activeTileTypes; + + /// Rotations of the active tiles + int[] activeTileRotations; + + /// Offsets along the Y axis of the active tiles + int[] activeTileOffsets; + + /// A flag for each tile that is set to true if it has been reloaded while batching is in progress + bool[] reloadedInBatch; + + /// + /// NavmeshCut and NavmeshAdd components registered to this tile handler. + /// This is updated by the class. + /// See: + /// + public readonly GridLookup cuts; + + /// + /// Positive while batching tile updates. + /// Batching tile updates has a positive effect on performance + /// + int batchDepth; + + /// + /// True while batching tile updates. + /// Batching tile updates has a positive effect on performance + /// + bool isBatching { get { return batchDepth > 0; } } + + /// + /// Utility for clipping polygons to rectangles. + /// Implemented as a struct and not a bunch of static methods + /// because it needs some buffer arrays that are best cached + /// to avoid excessive allocations + /// + // Note: Can technically be made readonly, but then C# will automatically copy the struct before every invocation + Voxelization.Int3PolygonClipper simpleClipper; + + /// + /// True if the tile handler still has the same number of tiles and tile layout as the graph. + /// If the graph is rescanned the tile handler will get out of sync and needs to be recreated. + /// + public bool isValid { + get { + return graph != null && graph.exists && tileXCount == graph.tileXCount && tileZCount == graph.tileZCount; + } + } + + public TileHandler (NavmeshBase graph) { + if (graph == null) throw new ArgumentNullException("graph"); + if (graph.GetTiles() == null) Debug.LogWarning("Creating a TileHandler for a graph with no tiles. Please scan the graph before creating a TileHandler"); + tileXCount = graph.tileXCount; + tileZCount = graph.tileZCount; + activeTileTypes = new TileType[tileXCount*tileZCount]; + activeTileRotations = new int[activeTileTypes.Length]; + activeTileOffsets = new int[activeTileTypes.Length]; + reloadedInBatch = new bool[activeTileTypes.Length]; + cuts = new GridLookup(new Int2(tileXCount, tileZCount)); + this.graph = graph; + } + + /// + /// Resize the tile handler to a different tile count. + /// See: + /// + public void Resize (IntRect newTileBounds) { + UnityEngine.Assertions.Assert.IsFalse(this.isBatching); + var newActiveTileTypes = new TileType[newTileBounds.Area]; + var newActiveTileRotations = new int[newActiveTileTypes.Length]; + var newActiveTileOffsets = new int[newActiveTileTypes.Length]; + var newReloadedInBatch = new bool[newActiveTileTypes.Length]; + for (int z = 0; z < tileZCount; z++) { + for (int x = 0; x < tileXCount; x++) { + if (newTileBounds.Contains(x, z)) { + var oldIndex = x + z*tileXCount; + var newIndex = (x - newTileBounds.xmin) + (z - newTileBounds.ymin)*newTileBounds.Width; + newActiveTileTypes[newIndex] = activeTileTypes[oldIndex]; + newActiveTileRotations[newIndex] = activeTileRotations[oldIndex]; + newActiveTileOffsets[newIndex] = activeTileOffsets[oldIndex]; + } + } + } + + this.tileXCount = newTileBounds.Width; + this.tileZCount = newTileBounds.Height; + this.activeTileTypes = newActiveTileTypes; + this.activeTileRotations = newActiveTileRotations; + this.activeTileOffsets = newActiveTileOffsets; + this.reloadedInBatch = newReloadedInBatch; + + for (int z = 0; z < tileZCount; z++) { + for (int x = 0; x < tileXCount; x++) { + var tileIndex = x + z*tileXCount; + if (activeTileTypes[tileIndex] == null) { + UpdateTileType(graph.GetTile(x, z)); + } + } + } + + this.cuts.Resize(newTileBounds); + } + + /// + /// Call to update the specified tiles with new information based on the navmesh/recast graph. + /// This is usually called right after a navmesh/recast graph has recalculated some tiles + /// and thus some calculations need to be done to take navmesh cutting into account + /// as well. + /// + /// Will reload all tiles in the list. + /// + public void OnRecalculatedTiles (NavmeshTile[] recalculatedTiles) { + for (int i = 0; i < recalculatedTiles.Length; i++) { + UpdateTileType(recalculatedTiles[i]); + } + + StartBatchLoad(); + + for (int i = 0; i < recalculatedTiles.Length; i++) { + ReloadTile(recalculatedTiles[i].x, recalculatedTiles[i].z); + } + + EndBatchLoad(); + } + + /// A template for a single tile in a navmesh/recast graph + public class TileType { + Int3[] verts; + int[] tris; + uint[] tags; + Int3 offset; + int lastYOffset; + int lastRotation; + int width; + int depth; + + public int Width { + get { + return width; + } + } + + public int Depth { + get { + return depth; + } + } + + /// + /// Matrices for rotation. + /// Each group of 4 elements is a 2x2 matrix. + /// The XZ position is multiplied by this. + /// So + /// + /// //A rotation by 90 degrees clockwise, second matrix in the array + /// (5,2) * ((0, 1), (-1, 0)) = (2,-5) + /// + /// + private static readonly int[] Rotations = { + 1, 0, // Identity matrix + 0, 1, + + 0, 1, + -1, 0, + + -1, 0, + 0, -1, + + 0, -1, + 1, 0 + }; + + public TileType (UnsafeSpan sourceVerts, UnsafeSpan sourceTris, uint[] tags, Int3 tileSize, Int3 centerOffset, int width = 1, int depth = 1) { + tris = sourceTris.ToArray(); + this.tags = tags; + + verts = new Int3[sourceVerts.Length]; + + offset = tileSize/2; + offset.x *= width; + offset.z *= depth; + offset.y = 0; + offset += centerOffset; + + for (int i = 0; i < sourceVerts.Length; i++) { + verts[i] = sourceVerts[i] + offset; + } + + lastRotation = 0; + lastYOffset = 0; + + this.width = width; + this.depth = depth; + } + + /// + /// Create a new TileType. + /// First all vertices of the source mesh are offseted by the centerOffset. + /// The source mesh is assumed to be centered (after offsetting). Corners of the tile should be at tileSize*0.5 along all axes. + /// When width or depth is not 1, the tileSize param should not change, but corners of the tile are assumed to lie further out. + /// + /// The navmesh as a unity Mesh + /// The number of base tiles this tile type occupies on the x-axis + /// The number of base tiles this tile type occupies on the z-axis + /// Size of a single tile, the y-coordinate will be ignored. + /// This offset will be added to all vertices + public TileType (Mesh source, Int3 tileSize, Int3 centerOffset, int width = 1, int depth = 1) { + if (source == null) throw new ArgumentNullException("source"); + + Vector3[] vectorVerts = source.vertices; + tris = source.triangles; + verts = new Int3[vectorVerts.Length]; + this.tags = null; + + for (int i = 0; i < vectorVerts.Length; i++) { + verts[i] = (Int3)vectorVerts[i] + centerOffset; + } + + offset = tileSize/2; + offset.x *= width; + offset.z *= depth; + offset.y = 0; + + for (int i = 0; i < vectorVerts.Length; i++) { + verts[i] = verts[i] + offset; + } + + lastRotation = 0; + lastYOffset = 0; + + this.width = width; + this.depth = depth; + } + + /// + /// Load a tile, result given by the vert and tris array. + /// Warning: For performance and memory reasons, the returned arrays are internal arrays, so they must not be modified in any way or + /// subsequent calls to Load may give corrupt output. The contents of the verts array is only valid until the next call to Load since + /// different rotations and y offsets can be applied. + /// If you need persistent arrays, please copy the returned ones. + /// + public void Load (out Int3[] verts, out int[] tris, out uint[] tags, int rotation, int yoffset) { + //Make sure it is a number 0 <= x < 4 + rotation = ((rotation % 4) + 4) % 4; + + //Figure out relative rotation (relative to previous rotation that is, since that is still applied to the verts array) + int tmp = rotation; + rotation = (rotation - (lastRotation % 4) + 4) % 4; + lastRotation = tmp; + + verts = this.verts; + + int relYOffset = yoffset - lastYOffset; + lastYOffset = yoffset; + + if (rotation != 0 || relYOffset != 0) { + for (int i = 0; i < verts.Length; i++) { + Int3 op = verts[i] - offset; + Int3 p = op; + p.y += relYOffset; + p.x = op.x * Rotations[rotation*4 + 0] + op.z * Rotations[rotation*4 + 1]; + p.z = op.x * Rotations[rotation*4 + 2] + op.z * Rotations[rotation*4 + 3]; + verts[i] = p + offset; + } + } + + tris = this.tris; + tags = this.tags; + } + } + + /// + /// Vertices and triangles used as input for the navmesh cutting. + /// + /// The vertices are in tile-space. So (0,0) is a corner of the tile. Distances are the same as in graph-space. + /// + /// Warning: For performance and memory reasons, the returned arrays are internal arrays, so they must not be modified in any way or + /// subsequent calls to Load may give corrupt output. The contents of the verts array is only valid until the next call to GetSourceTileData since + /// different rotations and y offsets can be applied. + /// If you need persistent arrays, please copy the returned ones. + /// + public void GetSourceTileData (int x, int z, out Int3[] verts, out int[] tris, out uint[] tags) { + var tileIndex = x + z*tileXCount; + this.activeTileTypes[tileIndex].Load(out verts, out tris, out tags, activeTileRotations[tileIndex], activeTileOffsets[tileIndex]); + } + + /// + /// Register that a tile can be loaded from source. + /// + /// Returns: Identifier for loading that tile type + /// + /// Assumes that the mesh has its pivot point at the center of the tile. + /// If it has not, you can supply a non-zero centerOffset to offset all vertices. + /// width of the tile. In base tiles, not world units. + /// depth of the tile. In base tiles, not world units. + /// Source mesh, must be readable. + public TileType RegisterTileType (Mesh source, Int3 centerOffset, int width = 1, int depth = 1) { + return new TileType(source, (Int3) new Vector3(graph.TileWorldSizeX, 0, graph.TileWorldSizeZ), centerOffset, width, depth); + } + + public void CreateTileTypesFromGraph () { + NavmeshTile[] tiles = graph.GetTiles(); + if (tiles == null) + return; + + if (!isValid) { + throw new InvalidOperationException("Graph tiles are invalid (number of tiles is not equal to width*depth of the graph). You need to create a new tile handler if you have changed the graph."); + } + + for (int z = 0; z < tileZCount; z++) { + for (int x = 0; x < tileXCount; x++) { + NavmeshTile tile = tiles[x + z*tileXCount]; + UpdateTileType(tile); + } + } + } + + void UpdateTileType (NavmeshTile tile) { + int x = tile.x; + int z = tile.z; + + Int3 size = (Int3) new Vector3(graph.TileWorldSizeX, 0, graph.TileWorldSizeZ); + Bounds b = graph.GetTileBoundsInGraphSpace(x, z); + var centerOffset = -((Int3)b.min + new Int3(size.x*tile.w/2, 0, size.z*tile.d/2)); + + var tags = new uint[tile.nodes.Length]; + for (int i = 0; i < tags.Length; i++) tags[i] = tile.nodes[i].Tag; + var tileType = new TileType(tile.vertsInGraphSpace, tile.tris, tags, size, centerOffset, tile.w, tile.d); + + int index = x + z*tileXCount; + + activeTileTypes[index] = tileType; + activeTileRotations[index] = 0; + activeTileOffsets[index] = 0; + } + + /// + /// Start batch loading. + /// Every call to this method must be matched by exactly one call to EndBatchLoad. + /// + public void StartBatchLoad () { + batchDepth++; + if (batchDepth > 1) return; + + AstarPath.active.AddWorkItem(new AstarWorkItem(force => { + graph.StartBatchTileUpdate(); + return true; + })); + } + + public void EndBatchLoad () { + if (batchDepth <= 0) throw new Exception("Ending batching when batching has not been started"); + batchDepth--; + + for (int i = 0; i < reloadedInBatch.Length; i++) reloadedInBatch[i] = false; + + AstarPath.active.AddWorkItem(new AstarWorkItem((ctx, force) => { + Profiler.BeginSample("Apply Tile Modifications"); + graph.EndBatchTileUpdate(); + Profiler.EndSample(); + return true; + })); + } + + [Flags] + public enum CutMode { + /// Cut holes in the navmesh + CutAll = 1, + /// Cut the navmesh but do not remove the interior of the cuts + CutDual = 2, + /// Also cut using the extra shape that was provided + CutExtra = 4 + } + + /// Internal class describing a single NavmeshCut + class Cut { + /// Bounds in XZ space + public IntRect bounds; + + /// X is the lower bound on the y axis, Y is the upper bounds on the Y axis + public Int2 boundsY; + public bool isDual; + public bool cutsAddedGeom; + public List contour; + } + + /// Internal class representing a mesh which is the result of the CutPoly method + struct CuttingResult { + public Int3[] verts; + public int[] tris; + public uint[] tags; + } + + /// + /// Cuts a piece of navmesh using navmesh cuts. + /// + /// Note: I am sorry for the really messy code in this method. + /// It really needs to be refactored. + /// + /// See: NavmeshBase.transform + /// See: CutMode + /// + /// Vertices that are going to be cut. Should be in graph space. + /// Triangles describing a mesh using the vertices. + /// Tags for each triangle. Will be passed to the resulting mesh. + /// If supplied the resulting mesh will be the intersection of the input mesh and this mesh. + /// Transform mapping graph space to world space. + /// Tiles in the recast graph which the mesh covers. + /// + /// Move navmesh cuts around randomly a bit, the larger the value the more they are moved around. + /// Used to prevent edge cases that can cause the clipping to fail. + CuttingResult CutPoly (Int3[] verts, int[] tris, uint[] tags, Int3[] extraShape, GraphTransform graphTransform, IntRect tiles, CutMode mode = CutMode.CutAll | CutMode.CutDual, int perturbate = -1) { + // Find all NavmeshAdd components that could be inside the bounds + List navmeshAdds = cuts.QueryRect(tiles); + + // Nothing to do here + if ((verts.Length == 0 || tris.Length == 0) && navmeshAdds.Count == 0) { + return new CuttingResult { + verts = ArrayPool.Claim(0), + tris = ArrayPool.Claim(0), + tags = ArrayPool.Claim(0), + }; + } + + if (perturbate > 10) { + Debug.LogError("Too many perturbations aborting.\n" + + "This may cause a tile in the navmesh to become empty. " + + "Try to see see if any of your NavmeshCut or NavmeshAdd components use invalid custom meshes."); + return new CuttingResult { + verts = verts, + tris = tris, + tags = tags, + }; + } + + List extraClipShape = null; + + // Do not cut with extra shape if there is no extra shape + if (extraShape == null && (mode & CutMode.CutExtra) != 0) { + throw new Exception("extraShape is null and the CutMode specifies that it should be used. Cannot use null shape."); + } + + // Calculate tile bounds so that the correct cutting offset can be used + // The tile will be cut in local space (i.e it is at the world origin) so cuts need to be translated + // to that point from their world space coordinates + var graphSpaceBounds = graph.GetTileBoundsInGraphSpace(tiles); + var cutOffset = graphSpaceBounds.min; + var transform = graphTransform * Matrix4x4.TRS(cutOffset, Quaternion.identity, Vector3.one); + // cutRegionSize The cutting region is a rectangle with one corner at the origin and one at the coordinates of cutRegionSize + // NavmeshAdd components will be clipped against this rectangle. It is assumed that the input vertices do not extend outside the region. + // For navmesh tiles, cutRegionSize is set to the size of a single tile. + var cutRegionSize = new Vector2(graphSpaceBounds.size.x, graphSpaceBounds.size.z); + var characterRadius = graph.NavmeshCuttingCharacterRadius; + + if ((mode & CutMode.CutExtra) != 0) { + extraClipShape = ListPool.Claim(extraShape.Length); + for (int i = 0; i < extraShape.Length; i++) { + var p = transform.InverseTransform(extraShape[i]); + extraClipShape.Add(new IntPoint(p.x, p.z)); + } + } + + // Find all NavmeshCut components that could be inside these bounds + List navmeshCuts; + if (mode == CutMode.CutExtra) { + // Not needed when only cutting extra + navmeshCuts = ListPool.Claim(); + } else { + navmeshCuts = cuts.QueryRect(tiles); + } + + var intersectingCuts = ListPool.Claim(); + + var cutInfos = PrepareNavmeshCutsForCutting(navmeshCuts, transform, perturbate, characterRadius); + + var outverts = ListPool.Claim(verts.Length*2); + var outtris = ListPool.Claim(tris.Length); + var outtags = ListPool.Claim(tags.Length); + + if (navmeshCuts.Count == 0 && navmeshAdds.Count == 0 && (mode & ~(CutMode.CutAll | CutMode.CutDual)) == 0 && (mode & CutMode.CutAll) != 0) { + // Fast path for the common case, no cuts or adds to the navmesh, so we just copy the vertices + CopyMesh(verts, tris, tags, outverts, outtris, outtags); + } else { + var poly = ListPool.Claim(); + var point2Index = new Dictionary(); + var polypoints = ListPool.Claim(); + + var clipResult = new Pathfinding.ClipperLib.PolyTree(); + var intermediateClipResult = ListPool >.Claim(); + var polyCache = StackPool.Claim(); + + // If we failed the previous iteration + // use a higher quality cutting + // this is heavier on the CPU, so only use it in special cases + clipper.StrictlySimple = perturbate > -1; + clipper.ReverseSolution = true; + + Int3[] clipIn = null; + Int3[] clipOut = null; + Int2 clipSize = new Int2(); + + if (navmeshAdds.Count > 0) { + clipIn = new Int3[7]; + clipOut = new Int3[7]; + // TODO: What if the size is odd? + // Convert cutRegionSize to an Int2 (all the casting is used to scale it appropriately, Int2 does not have an explicit conversion) + clipSize = new Int2(((Int3)(Vector3)cutRegionSize).x, ((Int3)(Vector3)cutRegionSize).y); + } + + // Iterate over all meshes that will make up the navmesh surface + Int3[] vertexBuffer = null; + for (int meshIndex = -1; meshIndex < navmeshAdds.Count; meshIndex++) { + // Current array of vertices and triangles that are being processed + Int3[] cverts; + int[] ctris; + uint[] ctags; + if (meshIndex == -1) { + cverts = verts; + ctris = tris; + ctags = tags; + } else { + navmeshAdds[meshIndex].GetMesh(ref vertexBuffer, out ctris, transform); + cverts = vertexBuffer; + ctags = null; + } + + for (int tri = 0; tri < ctris.Length; tri += 3) { + Int3 tp1 = cverts[ctris[tri + 0]]; + Int3 tp2 = cverts[ctris[tri + 1]]; + Int3 tp3 = cverts[ctris[tri + 2]]; + var tag = ctags != null ? ctags[tri/3] : 0; + + if (VectorMath.IsColinearXZ(tp1, tp2, tp3)) { + Debug.LogWarning("Skipping degenerate triangle."); + continue; + } + + var triBounds = new IntRect(tp1.x, tp1.z, tp1.x, tp1.z); + triBounds = triBounds.ExpandToContain(tp2.x, tp2.z); + triBounds = triBounds.ExpandToContain(tp3.x, tp3.z); + + // Upper and lower bound on the Y-axis, the above bounds do not have Y axis information + int tpYMin = Math.Min(tp1.y, Math.Min(tp2.y, tp3.y)); + int tpYMax = Math.Max(tp1.y, Math.Max(tp2.y, tp3.y)); + + intersectingCuts.Clear(); + bool hasDual = false; + + for (int i = 0; i < cutInfos.Count; i++) { + int ymin = cutInfos[i].boundsY.x; + int ymax = cutInfos[i].boundsY.y; + + if (IntRect.Intersects(triBounds, cutInfos[i].bounds) && !(ymax< tpYMin || ymin > tpYMax) && (cutInfos[i].cutsAddedGeom || meshIndex == -1)) { + Int3 p1 = tp1; + p1.y = ymin; + Int3 p2 = tp1; + p2.y = ymax; + + intersectingCuts.Add(i); + hasDual |= cutInfos[i].isDual; + } + } + + // Check if this is just a simple triangle which no navmesh cuts intersect and + // there are no other special things that should be done + if (intersectingCuts.Count == 0 && (mode & CutMode.CutExtra) == 0 && (mode & CutMode.CutAll) != 0 && meshIndex == -1) { + // Just add the triangle and be done with it + + // Refers to vertices to be added a few lines below + outtris.Add(outverts.Count + 0); + outtris.Add(outverts.Count + 1); + outtris.Add(outverts.Count + 2); + + outverts.Add(tp1); + outverts.Add(tp2); + outverts.Add(tp3); + + outtags.Add(tag); + continue; + } + + // Add current triangle as subject polygon for cutting + poly.Clear(); + if (meshIndex == -1) { + // Geometry from a tile mesh is assumed to be completely inside the tile + poly.Add(new IntPoint(tp1.x, tp1.z)); + poly.Add(new IntPoint(tp2.x, tp2.z)); + poly.Add(new IntPoint(tp3.x, tp3.z)); + } else { + // Added geometry must be clipped against the tile bounds + clipIn[0] = tp1; + clipIn[1] = tp2; + clipIn[2] = tp3; + + int ct = ClipAgainstRectangle(clipIn, clipOut, clipSize); + + // Check if triangle was completely outside the tile + if (ct == 0) { + continue; + } + + for (int q = 0; q < ct; q++) + poly.Add(new IntPoint(clipIn[q].x, clipIn[q].z)); + } + + point2Index.Clear(); + + // Loop through all possible modes + for (int cmode = 0; cmode < 4; cmode++) { + // Ignore modes which are not active + if ((((int)mode >> cmode) & 0x1) == 0) + continue; + + if (1 << cmode == (int)CutMode.CutAll) { + CutAll(poly, intersectingCuts, cutInfos, clipResult); + } else if (1 << cmode == (int)CutMode.CutDual) { + // No duals, don't bother processing this + if (!hasDual) + continue; + + CutDual(poly, intersectingCuts, cutInfos, hasDual, intermediateClipResult, clipResult); + } else if (1 << cmode == (int)CutMode.CutExtra) { + CutExtra(poly, extraClipShape, clipResult); + } + + for (int exp = 0; exp < clipResult.ChildCount; exp++) { + PolyNode node = clipResult.Childs[exp]; + List outer = node.Contour; + List holes = node.Childs; + + if (holes.Count == 0 && outer.Count == 3 && meshIndex == -1) { + for (int i = 0; i < 3; i++) { + var p = new Int3((int)outer[i].X, 0, (int)outer[i].Y); + p.y = Pathfinding.Polygon.SampleYCoordinateInTriangle(tp1, tp2, tp3, p); + + outtris.Add(outverts.Count); + outverts.Add(p); + } + outtags.Add(tag); + } else { + Poly2Tri.Polygon polygonToTriangulate = null; + // Loop over outer and all holes + int hole = -1; + List contour = outer; + while (contour != null) { + polypoints.Clear(); + for (int i = 0; i < contour.Count; i++) { + // Create a new point + var pp = new PolygonPoint(contour[i].X, contour[i].Y); + + // Add the point to the polygon + polypoints.Add(pp); + + var p = new Int3((int)contour[i].X, 0, (int)contour[i].Y); + p.y = Pathfinding.Polygon.SampleYCoordinateInTriangle(tp1, tp2, tp3, p); + + // Prepare a lookup table for pp -> vertex index + point2Index[pp] = outverts.Count; + + // Add to resulting vertex list + outverts.Add(p); + } + + Poly2Tri.Polygon contourPolygon = null; + if (polyCache.Count > 0) { + contourPolygon = polyCache.Pop(); + contourPolygon.AddPoints(polypoints); + } else { + contourPolygon = new Poly2Tri.Polygon(polypoints); + } + + // Since the outer contour is the first to be processed, polygonToTriangle will be null + // Holes are processed later, when polygonToTriangle is not null + if (hole == -1) { + polygonToTriangulate = contourPolygon; + } else { + polygonToTriangulate.AddHole(contourPolygon); + } + + hole++; + contour = hole < holes.Count ? holes[hole].Contour : null; + } + + // Triangulate the polygon with holes + try { + P2T.Triangulate(polygonToTriangulate); + } catch (Poly2Tri.PointOnEdgeException) { + Debug.LogWarning("PointOnEdgeException, perturbating vertices slightly.\nThis is usually fine. It happens sometimes because of rounding errors. Cutting will be retried a few more times."); + return CutPoly(verts, tris, tags, extraShape, graphTransform, tiles, mode, perturbate + 1); + } + + try { + for (int i = 0; i < polygonToTriangulate.Triangles.Count; i++) { + Poly2Tri.DelaunayTriangle t = polygonToTriangulate.Triangles[i]; + + // Add the triangle with the correct indices (using the previously built lookup table) + outtris.Add(point2Index[t.Points._0]); + outtris.Add(point2Index[t.Points._1]); + outtris.Add(point2Index[t.Points._2]); + outtags.Add(tag); + } + } catch (System.Collections.Generic.KeyNotFoundException) { + Debug.LogWarning("KeyNotFoundException, perturbating vertices slightly.\nThis is usually fine. It happens sometimes because of rounding errors. Cutting will be retried a few more times."); + return CutPoly(verts, tris, tags, extraShape, graphTransform, tiles, mode, perturbate + 1); + } + + PoolPolygon(polygonToTriangulate, polyCache); + } + } + } + } + } + + if (vertexBuffer != null) ArrayPool.Release(ref vertexBuffer); + StackPool.Release(polyCache); + ListPool >.Release(ref intermediateClipResult); + ListPool.Release(ref poly); + ListPool.Release(ref polypoints); + } + + // This next step will remove all duplicate vertices in the data (of which there are quite a few) + // and output the final vertex and triangle arrays to the outVertsArr and outTrisArr variables + var result = new CuttingResult(); + Pathfinding.Polygon.CompressMesh(outverts, outtris, outtags, out result.verts, out result.tris, out result.tags); + + // Notify the navmesh cuts that they were used + for (int i = 0; i < navmeshCuts.Count; i++) { + navmeshCuts[i].UsedForCut(); + } + + // Release back to pools + ListPool.Release(ref outverts); + ListPool.Release(ref outtris); + ListPool.Release(ref outtags); + ListPool.Release(ref intersectingCuts); + + for (int i = 0; i < cutInfos.Count; i++) { + ListPool.Release(cutInfos[i].contour); + } + + ListPool.Release(ref cutInfos); + ListPool.Release(ref navmeshCuts); + return result; + } + + /// + /// Generates a list of cuts from the navmesh cut components. + /// Each cut has a single contour (NavmeshCut components may contain multiple). + /// + /// transform should transform a point from cut space to world space. + /// + static List PrepareNavmeshCutsForCutting (List navmeshCuts, GraphTransform transform, int perturbate, float characterRadius) { + System.Random rnd = null; + if (perturbate > 0) { + rnd = new System.Random(); + } + + var contourVertices = new UnsafeList(0, Allocator.Temp); + var contours = new UnsafeList(0, Allocator.Temp); + var result = ListPool.Claim(); + for (int i = 0; i < navmeshCuts.Count; i++) { + // Generate random perturbation for this obstacle if required + Int2 perturbation = new Int2(0, 0); + if (perturbate > 0) { + // Create a perturbation vector, choose a point with coordinates in the set [-3*perturbate,3*perturbate] + // makes sure none of the coordinates are zero + + perturbation.x = (rnd.Next() % 6*perturbate) - 3*perturbate; + if (perturbation.x >= 0) perturbation.x++; + + perturbation.y = (rnd.Next() % 6*perturbate) - 3*perturbate; + if (perturbation.y >= 0) perturbation.y++; + } + + unsafe { + navmeshCuts[i].GetContourBurst(&contourVertices, &contours, transform.inverseMatrix, characterRadius); + } + + for (int j = 0; j < contours.Length; j++) { + NavmeshCut.ContourBurst contour = contours[j]; + + if (contour.endIndex <= contour.startIndex) { + Debug.LogError("A NavmeshCut component had a zero length contour. Ignoring that contour."); + continue; + } + + // TODO: transform should include cutting offset + List i3contour = ListPool.Claim(contour.endIndex - contour.startIndex); + for (int q = contour.startIndex; q < contour.endIndex; q++) { + var p = contourVertices[q] * Int3.FloatPrecision; + var ip = new IntPoint((long)p.x, (long)p.y); + if (perturbate > 0) { + ip.X += perturbation.x; + ip.Y += perturbation.y; + } + + i3contour.Add(ip); + } + + IntRect contourBounds = new IntRect((int)i3contour[0].X, (int)i3contour[0].Y, (int)i3contour[0].X, (int)i3contour[0].Y); + + for (int q = 0; q < i3contour.Count; q++) { + IntPoint p = i3contour[q]; + contourBounds = contourBounds.ExpandToContain((int)p.X, (int)p.Y); + } + + Cut cut = new Cut(); + + // Calculate bounds on the y axis + cut.boundsY = new Int2((int)(contour.ymin * Int3.FloatPrecision), (int)(contour.ymax * Int3.FloatPrecision)); + cut.bounds = contourBounds; + cut.isDual = navmeshCuts[i].isDual; + cut.cutsAddedGeom = navmeshCuts[i].cutsAddedGeom; + cut.contour = i3contour; + result.Add(cut); + } + + contours.Clear(); + contourVertices.Clear(); + } + + contours.Dispose(); + contourVertices.Dispose(); + return result; + } + + static void PoolPolygon (Poly2Tri.Polygon polygon, Stack pool) { + if (polygon.Holes != null) + for (int i = 0; i < polygon.Holes.Count; i++) { + polygon.Holes[i].Points.Clear(); + polygon.Holes[i].ClearTriangles(); + + if (polygon.Holes[i].Holes != null) + polygon.Holes[i].Holes.Clear(); + + pool.Push(polygon.Holes[i]); + } + polygon.ClearTriangles(); + if (polygon.Holes != null) + polygon.Holes.Clear(); + polygon.Points.Clear(); + pool.Push(polygon); + } + + void CutAll (List poly, List intersectingCutIndices, List cuts, Pathfinding.ClipperLib.PolyTree result) { + clipper.Clear(); + clipper.AddPolygon(poly, PolyType.ptSubject); + + // Add all holes (cuts) as clip polygons + // TODO: AddPolygon allocates quite a lot, modify ClipperLib to use object pooling + for (int i = 0; i < intersectingCutIndices.Count; i++) { + clipper.AddPolygon(cuts[intersectingCutIndices[i]].contour, PolyType.ptClip); + } + + result.Clear(); + clipper.Execute(ClipType.ctDifference, result, PolyFillType.pftNonZero, PolyFillType.pftNonZero); + } + + void CutDual (List poly, List tmpIntersectingCuts, List cuts, bool hasDual, List > intermediateResult, Pathfinding.ClipperLib.PolyTree result) { + // First calculate + // a = original intersection dualCuts + // then + // b = a difference normalCuts + // then process b as normal + clipper.Clear(); + clipper.AddPolygon(poly, PolyType.ptSubject); + + // Add all holes (cuts) as clip polygons + for (int i = 0; i < tmpIntersectingCuts.Count; i++) { + if (cuts[tmpIntersectingCuts[i]].isDual) { + clipper.AddPolygon(cuts[tmpIntersectingCuts[i]].contour, PolyType.ptClip); + } + } + + clipper.Execute(ClipType.ctIntersection, intermediateResult, PolyFillType.pftEvenOdd, PolyFillType.pftNonZero); + clipper.Clear(); + + if (intermediateResult != null) { + for (int i = 0; i < intermediateResult.Count; i++) { + clipper.AddPolygon(intermediateResult[i], Pathfinding.ClipperLib.Clipper.Orientation(intermediateResult[i]) ? PolyType.ptClip : PolyType.ptSubject); + } + } + + for (int i = 0; i < tmpIntersectingCuts.Count; i++) { + if (!cuts[tmpIntersectingCuts[i]].isDual) { + clipper.AddPolygon(cuts[tmpIntersectingCuts[i]].contour, PolyType.ptClip); + } + } + + result.Clear(); + clipper.Execute(ClipType.ctDifference, result, PolyFillType.pftEvenOdd, PolyFillType.pftNonZero); + } + + void CutExtra (List poly, List extraClipShape, Pathfinding.ClipperLib.PolyTree result) { + clipper.Clear(); + clipper.AddPolygon(poly, PolyType.ptSubject); + clipper.AddPolygon(extraClipShape, PolyType.ptClip); + + result.Clear(); + clipper.Execute(ClipType.ctIntersection, result, PolyFillType.pftEvenOdd, PolyFillType.pftNonZero); + } + + /// + /// Clips the input polygon against a rectangle with one corner at the origin and one at size in XZ space. + /// + /// Returns: Number of output vertices + /// + /// Input vertices + /// Output vertices. This buffer must be large enough to contain all output vertices. + /// The clipping rectangle has one corner at the origin and one at this position in XZ space. + int ClipAgainstRectangle (Int3[] clipIn, Int3[] clipOut, Int2 size) { + int ct; + + ct = simpleClipper.ClipPolygon(clipIn, 3, clipOut, 1, 0, 0); + if (ct == 0) + return ct; + + ct = simpleClipper.ClipPolygon(clipOut, ct, clipIn, -1, size.x, 0); + if (ct == 0) + return ct; + + ct = simpleClipper.ClipPolygon(clipIn, ct, clipOut, 1, 0, 2); + if (ct == 0) + return ct; + + ct = simpleClipper.ClipPolygon(clipOut, ct, clipIn, -1, size.y, 2); + return ct; + } + + /// Copy mesh from (vertices, triangles) to (outVertices, outTriangles) + static void CopyMesh (Int3[] vertices, int[] triangles, uint[] tags, List outVertices, List outTriangles, List outTags) { + outTriangles.Capacity = Math.Max(outTriangles.Capacity, triangles.Length); + outVertices.Capacity = Math.Max(outVertices.Capacity, vertices.Length); + outTags.Capacity = Math.Max(outTags.Capacity, tags.Length); + + for (int i = 0; i < vertices.Length; i++) { + outVertices.Add(vertices[i]); + } + + for (int i = 0; i < triangles.Length; i++) { + outTriangles.Add(triangles[i]); + } + + for (int i = 0; i < tags.Length; i++) { + outTags.Add(tags[i]); + } + } + + /// + /// Refine a mesh using delaunay refinement. + /// Loops through all pairs of neighbouring triangles and check if it would be better to flip the diagonal joining them + /// using the delaunay criteria. + /// + /// Does not require triangles to be clockwise, triangles will be checked for if they are clockwise and made clockwise if not. + /// The resulting mesh will have all triangles clockwise. + /// + /// See: https://en.wikipedia.org/wiki/Delaunay_triangulation + /// + void DelaunayRefinement (Int3[] verts, int[] tris, uint[] tags, ref int tCount, bool delaunay, bool colinear) { + if (tCount % 3 != 0) throw new System.ArgumentException("Triangle array length must be a multiple of 3"); + if (tags != null && tags.Length != tCount / 3) throw new System.ArgumentException("There must be exactly 1 tag per 3 triangle indices"); + + Dictionary lookup = cached_Int2_int_dict; + lookup.Clear(); + + for (int i = 0; i < tCount; i += 3) { + if (!VectorMath.IsClockwiseXZ(verts[tris[i]], verts[tris[i+1]], verts[tris[i+2]])) { + int tmp = tris[i]; + tris[i] = tris[i+2]; + tris[i+2] = tmp; + } + + lookup[new Int2(tris[i+0], tris[i+1])] = i+2; + lookup[new Int2(tris[i+1], tris[i+2])] = i+0; + lookup[new Int2(tris[i+2], tris[i+0])] = i+1; + } + + for (int i = 0; i < tCount; i += 3) { + var tag = tags != null ? tags[i/3] : 0; + for (int j = 0; j < 3; j++) { + int opp; + + if (lookup.TryGetValue(new Int2(tris[i+((j+1)%3)], tris[i+((j+0)%3)]), out opp)) { + // The vertex which we are using as the viewpoint + Int3 po = verts[tris[i+((j+2)%3)]]; + + // Right vertex of the edge + Int3 pr = verts[tris[i+((j+1)%3)]]; + + // Left vertex of the edge + Int3 pl = verts[tris[i+((j+3)%3)]]; + + // Opposite vertex (in the other triangle) + Int3 popp = verts[tris[opp]]; + + var oppTag = tags != null ? tags[opp/3] : 0; + + // Only allow flipping if the two adjacent triangles share the same tag + if (tag != oppTag) continue; + + po.y = 0; + pr.y = 0; + pl.y = 0; + popp.y = 0; + + bool noDelaunay = false; + + if (!VectorMath.RightOrColinearXZ(po, pl, popp) || VectorMath.RightXZ(po, pr, popp)) { + if (colinear) { + noDelaunay = true; + } else { + continue; + } + } + + if (colinear) { + const int MaxError = 3 * 3; + + // Check if op - right shared - opposite in other - is colinear + // and if the edge right-op is not shared and if the edge opposite in other - right shared is not shared + if (VectorMath.SqrDistancePointSegmentApproximate(po, popp, pr) < MaxError && + !lookup.ContainsKey(new Int2(tris[i+((j+2)%3)], tris[i+((j+1)%3)])) && + !lookup.ContainsKey(new Int2(tris[i+((j+1)%3)], tris[opp]))) { + tCount -= 3; + + int root = (opp/3)*3; + + // Move right vertex to the other triangle's opposite + tris[i+((j+1)%3)] = tris[opp]; + + // Remove the opposite triangle by swapping it with the last triangle + if (root != tCount) { + tris[root+0] = tris[tCount+0]; + tris[root+1] = tris[tCount+1]; + tris[root+2] = tris[tCount+2]; + tags[root/3] = tags[tCount/3]; + lookup[new Int2(tris[root+0], tris[root+1])] = root+2; + lookup[new Int2(tris[root+1], tris[root+2])] = root+0; + lookup[new Int2(tris[root+2], tris[root+0])] = root+1; + + tris[tCount+0] = 0; + tris[tCount+1] = 0; + tris[tCount+2] = 0; + } + + // Since the above mentioned edges are not shared, we don't need to bother updating them + + // However some need to be updated + // left - new right (previously opp) should have opposite vertex po + //lookup[new Int2(tris[i+((j+3)%3)],tris[i+((j+1)%3)])] = i+((j+2)%3); + + lookup[new Int2(tris[i+0], tris[i+1])] = i+2; + lookup[new Int2(tris[i+1], tris[i+2])] = i+0; + lookup[new Int2(tris[i+2], tris[i+0])] = i+1; + continue; + } + } + + if (delaunay && !noDelaunay) { + float beta = Int3.Angle(pr-po, pl-po); + float alpha = Int3.Angle(pr-popp, pl-popp); + + if (alpha > (2*Mathf.PI - 2*beta)) { + // Denaunay condition not holding, refine please + tris[i+((j+1)%3)] = tris[opp]; + + int root = (opp/3)*3; + int off = opp-root; + tris[root+((off-1+3) % 3)] = tris[i+((j+2)%3)]; + + lookup[new Int2(tris[i+0], tris[i+1])] = i+2; + lookup[new Int2(tris[i+1], tris[i+2])] = i+0; + lookup[new Int2(tris[i+2], tris[i+0])] = i+1; + + lookup[new Int2(tris[root+0], tris[root+1])] = root+2; + lookup[new Int2(tris[root+1], tris[root+2])] = root+0; + lookup[new Int2(tris[root+2], tris[root+0])] = root+1; + } + } + } + } + } + } + + /// Clear the tile at the specified tile coordinates + public void ClearTile (int x, int z) { + if (AstarPath.active == null) return; + + if (x < 0 || z < 0 || x >= tileXCount || z >= tileZCount) return; + + AstarPath.active.AddWorkItem(new AstarWorkItem((context, force) => { + //Replace the tile using the final vertices and triangles + graph.ReplaceTile(x, z, new Int3[0], new int[0]); + + activeTileTypes[x + z*tileXCount] = null; + + if (!isBatching) { + // Trigger post update event + // This can trigger for example recalculation of navmesh links + context.SetGraphDirty(graph); + } + + return true; + })); + } + + /// Reloads all tiles intersecting with the specified bounds + public void ReloadInBounds (Bounds bounds) { + ReloadInBounds(graph.GetTouchingTiles(bounds)); + } + + /// Reloads all tiles specified by the rectangle + public void ReloadInBounds (IntRect tiles) { + // Make sure the rect is inside graph bounds + tiles = IntRect.Intersection(tiles, new IntRect(0, 0, tileXCount-1, tileZCount-1)); + + if (!tiles.IsValid()) return; + + for (int z = tiles.ymin; z <= tiles.ymax; z++) { + for (int x = tiles.xmin; x <= tiles.xmax; x++) { + ReloadTile(x, z); + } + } + } + + /// + /// Reload tile at tile coordinate. + /// The last tile loaded at that position will be reloaded (e.g to account for moved NavmeshCut components) + /// + public void ReloadTile (int x, int z) { + if (x < 0 || z < 0 || x >= tileXCount || z >= tileZCount) return; + + int index = x + z*tileXCount; + if (activeTileTypes[index] != null) LoadTile(activeTileTypes[index], x, z, activeTileRotations[index], activeTileOffsets[index]); + } + + + /// Load a tile at tile coordinate x, z. + /// Tile type to load + /// Tile x coordinate (first tile is at (0,0), second at (1,0) etc.. ). + /// Tile z coordinate. + /// Rotate tile by 90 degrees * value. + /// Offset Y coordinates by this amount. In Int3 space, so if you have a world space + /// offset, multiply by Int3.Precision and round to the nearest integer before calling this function. + public void LoadTile (TileType tile, int x, int z, int rotation, int yoffset) { + if (tile == null) throw new ArgumentNullException("tile"); + + if (AstarPath.active == null) return; + + int index = x + z*tileXCount; + rotation = rotation % 4; + + // If loaded during this batch with the same settings, skip it + if (isBatching && reloadedInBatch[index] && activeTileOffsets[index] == yoffset && activeTileRotations[index] == rotation && activeTileTypes[index] == tile) { + return; + } + + reloadedInBatch[index] |= isBatching; + + activeTileOffsets[index] = yoffset; + activeTileRotations[index] = rotation; + activeTileTypes[index] = tile; + var originalSize = new Int2(this.tileXCount, this.tileZCount); + + // Add a work item + // This will pause pathfinding as soon as possible + // and call the delegate when it is safe to update graphs + AstarPath.active.AddWorkItem(new AstarWorkItem((context, force) => { + // If this was not the correct settings to load with, ignore + if (!(activeTileOffsets[index] == yoffset && activeTileRotations[index] == rotation && activeTileTypes[index] == tile)) return true; + // If the tile handler has been resized, ignore + if (originalSize != new Int2(this.tileXCount, this.tileZCount)) return true; + + context.PreUpdate(); + + tile.Load(out var verts, out var tris, out var tags, rotation, yoffset); + + Profiler.BeginSample("Cut Poly"); + // Cut the polygon + var tileBounds = new IntRect(x, z, x + tile.Width - 1, z + tile.Depth - 1); + var cuttingResult = CutPoly(verts, tris, tags, null, graph.transform, tileBounds); + Profiler.EndSample(); + + Profiler.BeginSample("Delaunay Refinement"); + // Refine to tweak bad triangles + var tCount = cuttingResult.tris.Length; + DelaunayRefinement(cuttingResult.verts, cuttingResult.tris, cuttingResult.tags, ref tCount, true, true); + Profiler.EndSample(); + + if (tCount != cuttingResult.tris.Length) { + cuttingResult.tris = Memory.ShrinkArray(cuttingResult.tris, tCount); + cuttingResult.tags = Memory.ShrinkArray(cuttingResult.tags, tCount/3); + } + + // Rotate the mask correctly + // and update width and depth to match rotation + // (width and depth will swap if rotated 90 or 270 degrees ) + int newWidth = rotation % 2 == 0 ? tile.Width : tile.Depth; + int newDepth = rotation % 2 == 0 ? tile.Depth : tile.Width; + + if (newWidth != 1 || newDepth != 1) throw new System.Exception("Only tiles of width = depth = 1 are supported at this time"); + + Profiler.BeginSample("ReplaceTile"); + // Replace the tile using the final vertices and triangles + // The vertices are still in local space + graph.ReplaceTile(x, z, cuttingResult.verts, cuttingResult.tris, cuttingResult.tags); + Profiler.EndSample(); + return true; + })); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileHandler.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileHandler.cs.meta new file mode 100644 index 0000000..ff3dc5d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileHandler.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 664ab28b7671144dfa4515ea79a4c49e +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileLayout.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileLayout.cs new file mode 100644 index 0000000..1e648d7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileLayout.cs @@ -0,0 +1,96 @@ +using UnityEngine; +using Pathfinding.Util; +using UnityEngine.Tilemaps; + +namespace Pathfinding.Graphs.Navmesh { + /// + /// Represents the position and size of a tile grid for a recast/navmesh graph. + /// + /// This separates out the physical layout of tiles from all the other recast graph settings. + /// + public struct TileLayout { + /// How many tiles there are in the grid + public Int2 tileCount; + + /// Transforms coordinates from graph space to world space + public GraphTransform transform; + + /// Size of a tile in voxels along the X and Z axes + public Int2 tileSizeInVoxels; + + /// + /// Size in graph space of the whole grid. + /// + /// If the original bounding box was not an exact multiple of the tile size, this will be less than the total width of all tiles. + /// + public Vector3 graphSpaceSize; + + /// \copydocref{RecastGraph.cellSize} + public float cellSize; + + /// + /// Voxel y coordinates will be stored as ushorts which have 65536 values. + /// Leave a margin to make sure things do not overflow + /// + public float CellHeight => Mathf.Max(graphSpaceSize.y / 64000, 0.001f); + + /// Size of a tile in world units, along the graph's X axis + public float TileWorldSizeX => tileSizeInVoxels.x * cellSize; + + /// Size of a tile in world units, along the graph's Z axis + public float TileWorldSizeZ => tileSizeInVoxels.y * cellSize; + + /// Returns an XZ bounds object with the bounds of a group of tiles in graph space + public Bounds GetTileBoundsInGraphSpace (int x, int z, int width = 1, int depth = 1) { + var bounds = new Bounds(); + + bounds.SetMinMax(new Vector3(x*TileWorldSizeX, 0, z*TileWorldSizeZ), + new Vector3((x+width)*TileWorldSizeX, graphSpaceSize.y, (z+depth)*TileWorldSizeZ) + ); + + return bounds; + } + + /// + /// Returns a rect containing the indices of all tiles touching the specified bounds. + /// If a margin is passed, the bounding box in graph space is expanded by that amount in every direction. + /// + public IntRect GetTouchingTiles (Bounds bounds, float margin = 0) { + bounds = transform.InverseTransform(bounds); + + // Calculate world bounds of all affected tiles + return new IntRect(Mathf.FloorToInt((bounds.min.x - margin) / TileWorldSizeX), Mathf.FloorToInt((bounds.min.z - margin) / TileWorldSizeZ), Mathf.FloorToInt((bounds.max.x + margin) / TileWorldSizeX), Mathf.FloorToInt((bounds.max.z + margin) / TileWorldSizeZ)); + } + + public TileLayout(RecastGraph graph) : this(new Bounds(graph.forcedBoundsCenter, graph.forcedBoundsSize), Quaternion.Euler(graph.rotation), graph.cellSize, graph.editorTileSize, graph.useTiles) { + } + + public TileLayout(Bounds bounds, Quaternion rotation, float cellSize, int tileSizeInVoxels, bool useTiles) { + this.transform = RecastGraph.CalculateTransform(bounds, rotation); + this.cellSize = cellSize; + + // Voxel grid size + var size = bounds.size; + graphSpaceSize = size; + int totalVoxelWidth = (int)(size.x/cellSize + 0.5f); + int totalVoxelDepth = (int)(size.z/cellSize + 0.5f); + + if (!useTiles) { + this.tileSizeInVoxels = new Int2(totalVoxelWidth, totalVoxelDepth); + } else { + this.tileSizeInVoxels = new Int2(tileSizeInVoxels, tileSizeInVoxels); + } + + // Number of tiles + tileCount = new Int2( + Mathf.Max(0, (totalVoxelWidth + this.tileSizeInVoxels.x-1) / this.tileSizeInVoxels.x), + Mathf.Max(0, (totalVoxelDepth + this.tileSizeInVoxels.y-1) / this.tileSizeInVoxels.y) + ); + + if (tileCount.x*tileCount.y > NavmeshBase.TileIndexMask + 1) { + throw new System.Exception("Too many tiles ("+(tileCount.x*tileCount.y)+") maximum is "+(NavmeshBase.TileIndexMask + 1)+ + "\nTry disabling ASTAR_RECAST_LARGER_TILES under the 'Optimizations' tab in the A* inspector."); + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileLayout.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileLayout.cs.meta new file mode 100644 index 0000000..f2886d6 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileLayout.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cf2ae7ff6aabbdc4fa76468eedbf53f6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMesh.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMesh.cs new file mode 100644 index 0000000..e5a772a --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMesh.cs @@ -0,0 +1,41 @@ +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Navmesh { + /// + /// A tile in a navmesh graph. + /// + /// This is an intermediate representation used when building the navmesh, and also in some cases for serializing the navmesh to a portable format. + /// + /// See: for the representation used for pathfinding. + /// + public struct TileMesh { + public int[] triangles; + public Int3[] verticesInTileSpace; + /// One tag per triangle + public uint[] tags; + + /// Unsafe version of + public struct TileMeshUnsafe { + /// Three indices per triangle, of type int + public Unity.Collections.LowLevel.Unsafe.UnsafeAppendBuffer triangles; + /// One vertex per triangle, of type Int3 + public Unity.Collections.LowLevel.Unsafe.UnsafeAppendBuffer verticesInTileSpace; + /// One tag per triangle, of type uint + public Unity.Collections.LowLevel.Unsafe.UnsafeAppendBuffer tags; + + public void Dispose () { + triangles.Dispose(); + verticesInTileSpace.Dispose(); + tags.Dispose(); + } + + public TileMesh ToManaged () { + return new TileMesh { + triangles = Memory.UnsafeAppendBufferToArray(triangles), + verticesInTileSpace = Memory.UnsafeAppendBufferToArray(verticesInTileSpace), + tags = Memory.UnsafeAppendBufferToArray(tags), + }; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMesh.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMesh.cs.meta new file mode 100644 index 0000000..a4f8d41 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMesh.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 16f2efac26c436946b764d2263a0a089 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMeshes.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMeshes.cs new file mode 100644 index 0000000..b1994fe --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMeshes.cs @@ -0,0 +1,193 @@ +using Unity.Collections; +using Unity.Mathematics; +using UnityEngine; + +namespace Pathfinding.Graphs.Navmesh { + /// + /// Represents a rectangular group of tiles of a recast graph. + /// + /// This is a portable representation in that it can be serialized to and from a byte array. + /// + /// + /// // Scans the first 6x6 chunk of tiles of the recast graph (the IntRect uses inclusive coordinates) + /// var graph = AstarPath.active.data.recastGraph; + /// var buildSettings = RecastBuilder.BuildTileMeshes(graph, new TileLayout(graph), new IntRect(0, 0, 5, 5)); + /// var disposeArena = new Pathfinding.Jobs.DisposeArena(); + /// var promise = buildSettings.Schedule(disposeArena); + /// + /// AstarPath.active.AddWorkItem(() => { + /// // Block until the asynchronous job completes + /// var result = promise.Complete(); + /// TileMeshes tiles = result.tileMeshes.ToManaged(); + /// // Take the scanned tiles and place them in the graph, + /// // but not at their original location, but 2 tiles away, rotated 90 degrees. + /// tiles.tileRect = tiles.tileRect.Offset(new Int2(2, 0)); + /// tiles.Rotate(1); + /// graph.ReplaceTiles(tiles); + /// + /// // Dispose unmanaged data + /// disposeArena.DisposeAll(); + /// result.Dispose(); + /// }); + /// + /// + /// See: uses this representation internally for storage. + /// See: + /// See: + /// + public struct TileMeshes { + /// Tiles laid out row by row + public TileMesh[] tileMeshes; + /// Which tiles in the graph this group of tiles represents + public IntRect tileRect; + /// World-space size of each tile + public Vector2 tileWorldSize; + + /// Rotate this group of tiles by 90*N degrees clockwise about the group's center + public void Rotate (int rotation) { + rotation = -rotation; + // Get the positive remainder modulo 4. I.e. a number between 0 and 3. + rotation = ((rotation % 4) + 4) % 4; + if (rotation == 0) return; + var rot90 = new int2x2(0, -1, 1, 0); + var rotN = int2x2.identity; + for (int i = 0; i < rotation; i++) rotN = math.mul(rotN, rot90); + + var tileSize = (Int3) new Vector3(tileWorldSize.x, 0, tileWorldSize.y); + var offset = -math.min(int2.zero, math.mul(rotN, new int2(tileSize.x, tileSize.z))); + var size = new int2(tileRect.Width, tileRect.Height); + var offsetTileCoordinate = -math.min(int2.zero, math.mul(rotN, size - 1)); + var newTileMeshes = new TileMesh[tileMeshes.Length]; + var newSize = (rotation % 2) == 0 ? size : new int2(size.y, size.x); + + for (int z = 0; z < size.y; z++) { + for (int x = 0; x < size.x; x++) { + var vertices = tileMeshes[x + z*size.x].verticesInTileSpace; + for (int i = 0; i < vertices.Length; i++) { + var v = vertices[i]; + var rotated = math.mul(rotN, new int2(v.x, v.z)) + offset; + vertices[i] = new Int3(rotated.x, v.y, rotated.y); + } + + var tileCoord = math.mul(rotN, new int2(x, z)) + offsetTileCoordinate; + newTileMeshes[tileCoord.x + tileCoord.y*newSize.x] = tileMeshes[x + z*size.x]; + } + } + + tileMeshes = newTileMeshes; + tileWorldSize = rotation % 2 == 0 ? tileWorldSize : new Vector2(tileWorldSize.y, tileWorldSize.x); + tileRect = new IntRect(tileRect.xmin, tileRect.ymin, tileRect.xmin + newSize.x - 1, tileRect.ymin + newSize.y - 1); + } + + /// + /// Serialize this struct to a portable byte array. + /// The data is compressed using the deflate algorithm to reduce size. + /// See: + /// + public byte[] Serialize () { + var buffer = new System.IO.MemoryStream(); + var writer = new System.IO.BinaryWriter(new System.IO.Compression.DeflateStream(buffer, System.IO.Compression.CompressionMode.Compress)); + // Version + writer.Write(0); + writer.Write(tileRect.Width); + writer.Write(tileRect.Height); + writer.Write(this.tileWorldSize.x); + writer.Write(this.tileWorldSize.y); + for (int z = 0; z < tileRect.Height; z++) { + for (int x = 0; x < tileRect.Width; x++) { + var tile = tileMeshes[(z*tileRect.Width) + x]; + UnityEngine.Assertions.Assert.IsTrue(tile.tags.Length*3 == tile.triangles.Length); + writer.Write(tile.triangles.Length); + writer.Write(tile.verticesInTileSpace.Length); + for (int i = 0; i < tile.verticesInTileSpace.Length; i++) { + var v = tile.verticesInTileSpace[i]; + writer.Write(v.x); + writer.Write(v.y); + writer.Write(v.z); + } + for (int i = 0; i < tile.triangles.Length; i++) writer.Write(tile.triangles[i]); + for (int i = 0; i < tile.tags.Length; i++) writer.Write(tile.tags[i]); + } + } + writer.Close(); + return buffer.ToArray(); + } + + /// + /// Deserialize an instance from a byte array. + /// See: + /// + public static TileMeshes Deserialize (byte[] bytes) { + var reader = new System.IO.BinaryReader(new System.IO.Compression.DeflateStream(new System.IO.MemoryStream(bytes), System.IO.Compression.CompressionMode.Decompress)); + var version = reader.ReadInt32(); + if (version != 0) throw new System.Exception("Invalid data. Unexpected version number."); + var w = reader.ReadInt32(); + var h = reader.ReadInt32(); + var tileSize = new Vector2(reader.ReadSingle(), reader.ReadSingle()); + if (w < 0 || h < 0) throw new System.Exception("Invalid bounds"); + + var tileRect = new IntRect(0, 0, w - 1, h - 1); + + var tileMeshes = new TileMesh[w*h]; + for (int z = 0; z < h; z++) { + for (int x = 0; x < w; x++) { + int[] tris = new int[reader.ReadInt32()]; + Int3[] vertsInTileSpace = new Int3[reader.ReadInt32()]; + uint[] tags = new uint[tris.Length/3]; + + for (int i = 0; i < vertsInTileSpace.Length; i++) vertsInTileSpace[i] = new Int3(reader.ReadInt32(), reader.ReadInt32(), reader.ReadInt32()); + for (int i = 0; i < tris.Length; i++) { + tris[i] = reader.ReadInt32(); + UnityEngine.Assertions.Assert.IsTrue(tris[i] >= 0 && tris[i] < vertsInTileSpace.Length); + } + for (int i = 0; i < tags.Length; i++) tags[i] = reader.ReadUInt32(); + + tileMeshes[x + z*w] = new TileMesh { + triangles = tris, + verticesInTileSpace = vertsInTileSpace, + tags = tags, + }; + } + } + return new TileMeshes { + tileMeshes = tileMeshes, + tileRect = tileRect, + tileWorldSize = tileSize, + }; + } + } + + /// Unsafe representation of a struct + public struct TileMeshesUnsafe { + public NativeArray tileMeshes; + public IntRect tileRect; + public Vector2 tileWorldSize; + + public TileMeshesUnsafe(NativeArray tileMeshes, IntRect tileRect, Vector2 tileWorldSize) { + this.tileMeshes = tileMeshes; + this.tileRect = tileRect; + this.tileWorldSize = tileWorldSize; + } + + /// Copies the native data to managed data arrays which are easier to work with + public TileMeshes ToManaged () { + var output = new TileMesh[tileMeshes.Length]; + for (int i = 0; i < output.Length; i++) { + output[i] = tileMeshes[i].ToManaged(); + } + return new TileMeshes { + tileMeshes = output, + tileRect = this.tileRect, + tileWorldSize = this.tileWorldSize, + }; + } + + public void Dispose () { + // Allows calling Dispose on zero-initialized instances + if (!tileMeshes.IsCreated) return; + + for (int i = 0; i < tileMeshes.Length; i++) tileMeshes[i].Dispose(); + tileMeshes.Dispose(); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMeshes.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMeshes.cs.meta new file mode 100644 index 0000000..8b94c61 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/TileMeshes.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8e1e88f7c3e2d2c45ab0ba43bbce2cd4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels.meta new file mode 100644 index 0000000..622bdfe --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: ce1c1f6432f234a46b5e914d99379d70 diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs new file mode 100644 index 0000000..1d8571e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs @@ -0,0 +1,132 @@ +using Pathfinding.Jobs; +using Unity.Collections; +using Unity.Mathematics; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + /// Stores a compact voxel field. + public struct CompactVoxelField : IArenaDisposable { + public const int UnwalkableArea = 0; + public const uint NotConnected = 0x3f; + public readonly int voxelWalkableHeight; + public readonly int width, depth; + public NativeList spans; + public NativeList cells; + public NativeList areaTypes; + + /// Unmotivated variable, but let's clamp the layers at 65535 + public const int MaxLayers = 65535; + + public CompactVoxelField (int width, int depth, int voxelWalkableHeight, Allocator allocator) { + spans = new NativeList(0, allocator); + cells = new NativeList(0, allocator); + areaTypes = new NativeList(0, allocator); + this.width = width; + this.depth = depth; + this.voxelWalkableHeight = voxelWalkableHeight; + } + + void IArenaDisposable.DisposeWith (DisposeArena arena) { + arena.Add(spans); + arena.Add(cells); + arena.Add(areaTypes); + } + + public int GetNeighbourIndex (int index, int direction) { + return index + VoxelUtilityBurst.DX[direction] + VoxelUtilityBurst.DZ[direction] * width; + } + + public void BuildFromLinkedField (LinkedVoxelField field) { + int idx = 0; + + Assert.AreEqual(this.width, field.width); + Assert.AreEqual(this.depth, field.depth); + + int w = field.width; + int d = field.depth; + int wd = w*d; + + int spanCount = field.GetSpanCount(); + spans.Resize(spanCount, NativeArrayOptions.UninitializedMemory); + areaTypes.Resize(spanCount, NativeArrayOptions.UninitializedMemory); + cells.Resize(wd, NativeArrayOptions.UninitializedMemory); + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (this.voxelWalkableHeight >= ushort.MaxValue) { + throw new System.Exception("Too high walkable height to guarantee correctness. Increase voxel height or lower walkable height."); + } +#endif + + var linkedSpans = field.linkedSpans; + for (int z = 0; z < wd; z += w) { + for (int x = 0; x < w; x++) { + int spanIndex = x+z; + if (linkedSpans[spanIndex].bottom == LinkedVoxelField.InvalidSpanValue) { + cells[x+z] = new CompactVoxelCell(0, 0); + continue; + } + + int index = idx; + int count = 0; + + while (spanIndex != -1) { + if (linkedSpans[spanIndex].area != UnwalkableArea) { + int bottom = (int)linkedSpans[spanIndex].top; + int next = linkedSpans[spanIndex].next; + int top = next != -1 ? (int)linkedSpans[next].bottom : LinkedVoxelField.MaxHeightInt; + + // TODO: Why is top-bottom clamped to a ushort range? + spans[idx] = new CompactVoxelSpan((ushort)math.min(bottom, ushort.MaxValue), (uint)math.min(top-bottom, ushort.MaxValue)); + areaTypes[idx] = linkedSpans[spanIndex].area; + idx++; + count++; + } + spanIndex = linkedSpans[spanIndex].next; + } + + cells[x+z] = new CompactVoxelCell(index, count); + } + } + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (idx != spanCount) throw new System.Exception("Found span count does not match expected value"); +#endif + } + } + + /// CompactVoxelCell used for recast graphs. + public struct CompactVoxelCell { + public int index; + public int count; + + public CompactVoxelCell (int i, int c) { + index = i; + count = c; + } + } + + /// CompactVoxelSpan used for recast graphs. + public struct CompactVoxelSpan { + public ushort y; + public uint con; + public uint h; + public int reg; + + public CompactVoxelSpan (ushort bottom, uint height) { + con = 24; + y = bottom; + h = height; + reg = 0; + } + + public void SetConnection (int dir, uint value) { + int shift = dir*6; + + con = (uint)((con & ~(0x3f << shift)) | ((value & 0x3f) << shift)); + } + + public int GetConnection (int dir) { + return ((int)con >> dir*6) & 0x3f; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs.meta new file mode 100644 index 0000000..d833992 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/CompactVoxelField.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ddc46f5b05337b6ba8eae5dd4906634d +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs new file mode 100644 index 0000000..fa2e2cd --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs @@ -0,0 +1,295 @@ +using Pathfinding.Jobs; +using Unity.Collections; +using Unity.Mathematics; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + struct CellMinMax { + public int objectID; + public int min; + public int max; + } + + public struct LinkedVoxelField : IArenaDisposable { + public const uint MaxHeight = 65536; + public const int MaxHeightInt = 65536; + /// + /// Constant for default LinkedVoxelSpan top and bottom values. + /// It is important with the U since ~0 != ~0U + /// This can be used to check if a LinkedVoxelSpan is valid and not just the default span + /// + public const uint InvalidSpanValue = ~0U; + + /// Initial estimate on the average number of spans (layers) in the voxel representation. Should be greater or equal to 1 + public const float AvgSpanLayerCountEstimate = 8; + + /// The width of the field along the x-axis. [Limit: >= 0] [Units: voxels] + public int width; + + /// The depth of the field along the z-axis. [Limit: >= 0] [Units: voxels] + public int depth; + /// The maximum height coordinate. [Limit: >= 0, <= MaxHeight] [Units: voxels] + public int height; + public bool flatten; + + public NativeList linkedSpans; + private NativeList removedStack; + private NativeList linkedCellMinMax; + + public LinkedVoxelField (int width, int depth, int height) { + this.width = width; + this.depth = depth; + this.height = height; + this.flatten = true; + linkedSpans = new NativeList(0, Allocator.Persistent); + removedStack = new NativeList(128, Allocator.Persistent); + linkedCellMinMax = new NativeList(0, Allocator.Persistent); + } + + void IArenaDisposable.DisposeWith (DisposeArena arena) { + arena.Add(linkedSpans); + arena.Add(removedStack); + arena.Add(linkedCellMinMax); + } + + public void ResetLinkedVoxelSpans () { + int len = width * depth; + + LinkedVoxelSpan df = new LinkedVoxelSpan(InvalidSpanValue, InvalidSpanValue, -1, -1); + + linkedSpans.ResizeUninitialized(len); + linkedCellMinMax.Resize(len, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < len; i++) { + linkedSpans[i] = df; + linkedCellMinMax[i] = new CellMinMax { + objectID = -1, + min = 0, + max = 0, + }; + } + removedStack.Clear(); + } + + void PushToSpanRemovedStack (int index) { + removedStack.Add(index); + } + + public int GetSpanCount () { + int count = 0; + + int wd = width*depth; + + for (int x = 0; x < wd; x++) { + for (int s = x; s != -1 && linkedSpans[s].bottom != InvalidSpanValue; s = linkedSpans[s].next) { + count += linkedSpans[s].area != 0 ? 1 : 0; + } + } + return count; + } + + public void ResolveSolid (int index, int objectID, int voxelWalkableClimb) { + var minmax = linkedCellMinMax[index]; + + if (minmax.objectID != objectID) return; + + if (minmax.min < minmax.max - 1) { + // Add a span for the solid part of the object. + // + // This span ends at max-1 (where max is the top of the original object). + // This is to avoid issues when merging spans with different areas. + // Assume we had 3 spans like: + // y=0..5 walkable span from another object, area=2 + // y=9..10 walkable span, area=3 + // and min=0, max=10 for the current object. + // If we added a span for the whole solid range (0..10), then it will first get merged with the 0..5 span, receiving its area (assuming walkable climb was high enough), + // and then get merged with the 9..10 span, replacing its area. This would make the final area be 2, instead of 3 like it should be. + // If we instead add a solid span for the range 0..9, then the tie breaking will ensure that the final area is 3. + // Spans are always at least 1 voxel tall, so the solid span will always get merged with the original span. + AddLinkedSpan(index, minmax.min, minmax.max-1, CompactVoxelField.UnwalkableArea, voxelWalkableClimb, objectID); + } + } + + public void SetWalkableBackground () { + int wd = width*depth; + + for (int i = 0; i < wd; i++) { + linkedSpans[i] = new LinkedVoxelSpan(0, 1, 1); + } + } + + public void AddFlattenedSpan (int index, int area) { + if (linkedSpans[index].bottom == InvalidSpanValue) { + linkedSpans[index] = new LinkedVoxelSpan(0, 1, area); + } else { + // The prioritized area is (in order): + // - the unwalkable area (area=0) + // - the higher valued area + linkedSpans[index] = new LinkedVoxelSpan(0, 1, linkedSpans[index].area == 0 || area == 0 ? 0 : math.max(linkedSpans[index].area, area)); + } + } + + public void AddLinkedSpan (int index, int bottom, int top, int area, int voxelWalkableClimb, int objectID) { + var minmax = linkedCellMinMax[index]; + + if (minmax.objectID != objectID) { + linkedCellMinMax[index] = new CellMinMax { + objectID = objectID, + min = bottom, + max = top, + }; + } else { + minmax.min = math.min(minmax.min, bottom); + minmax.max = math.max(minmax.max, top); + linkedCellMinMax[index] = minmax; + } + + // Clamp to bounding box. If the span was outside the bbox, then bottom will become greater than top. + top = math.min(top, height); + bottom = math.max(bottom, 0); + + // Skip span if below or above the bounding box or if the span is zero voxels tall + if (bottom >= top) return; + + var utop = (uint)top; + var ubottom = (uint)bottom; + + // linkedSpans[index] is the span with the lowest y-coordinate at the position x,z such that index=x+z*width + // i.e linkedSpans is a 2D array laid out in a 1D array (for performance and simplicity) + + // Check if there is a root span, otherwise we can just add a new (valid) span and exit + if (linkedSpans[index].bottom == InvalidSpanValue) { + linkedSpans[index] = new LinkedVoxelSpan(ubottom, utop, area); + return; + } + + int prev = -1; + + // Original index, the first span we visited + int oindex = index; + + while (index != -1) { + var current = linkedSpans[index]; + if (current.bottom > utop) { + // If the current span's bottom higher up than the span we want to insert's top, then they do not intersect + // and we should just insert a new span here + break; + } else if (current.top < ubottom) { + // The current span and the span we want to insert do not intersect + // so just skip to the next span (it might intersect) + prev = index; + index = current.next; + } else { + // Intersection! Merge the spans + + // If two spans have almost the same upper y coordinate then + // we don't just pick the area from the topmost span. + // Instead we pick the maximum of the two areas. + // This ensures that unwalkable spans that end up at the same y coordinate + // as a walkable span (very common for vertical surfaces that meet a walkable surface at a ledge) + // do not end up making the surface unwalkable. + // This is also important for larger distances when there are very small obstacles on the ground. + // For example if a small rock happened to have a surface that was greater than the max slope angle, + // then its surface would be unwalkable. Without this check, even if the rock was tiny, it would + // create a hole in the navmesh. + + // voxelWalkableClimb is flagMergeDistance, when a walkable flag is favored before an unwalkable one + // So if a walkable span intersects an unwalkable span, the walkable span can be up to voxelWalkableClimb + // below the unwalkable span and the merged span will still be walkable. + // If both spans are walkable we use the area from the topmost span. + if (math.abs((int)utop - (int)current.top) < voxelWalkableClimb && (area == CompactVoxelField.UnwalkableArea || current.area == CompactVoxelField.UnwalkableArea)) { + // linkedSpans[index] is the lowest span, but we might use that span's area anyway if it is walkable + area = math.max(area, current.area); + } else { + // Pick the area from the topmost span + if (utop < current.top) area = current.area; + } + + // Find the new bottom and top for the merged span + ubottom = math.min(current.bottom, ubottom); + utop = math.max(current.top, utop); + + // Find the next span in the linked list + int next = current.next; + if (prev != -1) { + // There is a previous span + // Remove this span from the linked list + // TODO: Kinda slow. Check what asm is generated. + var p = linkedSpans[prev]; + p.next = next; + linkedSpans[prev] = p; + + // Add this span index to a list for recycling + PushToSpanRemovedStack(index); + + // Move to the next span in the list + index = next; + } else if (next != -1) { + // This was the root span and there is a span left in the linked list + // Remove this span from the linked list by assigning the next span as the root span + linkedSpans[oindex] = linkedSpans[next]; + + // Recycle the old span index + PushToSpanRemovedStack(next); + + // Move to the next span in the list + // NOP since we just removed the current span, the next span + // we want to visit will have the same index as we are on now (i.e oindex) + } else { + // This was the root span and there are no other spans in the linked list + // Just replace the root span with the merged span and exit + linkedSpans[oindex] = new LinkedVoxelSpan(ubottom, utop, area); + return; + } + } + } + + // We now have a merged span that needs to be inserted + // and connected with the existing spans + + // The new merged span will be inserted right after 'prev' (if it exists, otherwise before index) + + // Take a node from the recycling stack if possible + // Otherwise create a new node (well, just a new index really) + int nextIndex; + if (removedStack.Length > 0) { + // Pop + nextIndex = removedStack[removedStack.Length - 1]; + removedStack.RemoveAtSwapBack(removedStack.Length - 1); + } else { + nextIndex = linkedSpans.Length; + linkedSpans.Resize(linkedSpans.Length + 1, NativeArrayOptions.UninitializedMemory); + } + + if (prev != -1) { + linkedSpans[nextIndex] = new LinkedVoxelSpan(ubottom, utop, area, linkedSpans[prev].next); + // TODO: Check asm + var p = linkedSpans[prev]; + p.next = nextIndex; + linkedSpans[prev] = p; + } else { + linkedSpans[nextIndex] = linkedSpans[oindex]; + linkedSpans[oindex] = new LinkedVoxelSpan(ubottom, utop, area, nextIndex); + } + } + } + + public struct LinkedVoxelSpan { + public uint bottom; + public uint top; + + public int next; + + /*Area + * 0 is an unwalkable span (triangle face down) + * 1 is a walkable span (triangle face up) + */ + public int area; + + public LinkedVoxelSpan (uint bottom, uint top, int area) { + this.bottom = bottom; this.top = top; this.area = area; this.next = -1; + } + + public LinkedVoxelSpan (uint bottom, uint top, int area, int next) { + this.bottom = bottom; this.top = top; this.area = area; this.next = next; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs.meta new file mode 100644 index 0000000..defeb4a --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/LinkedVoxelField.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b6e41a3dcfac38cd8910584fc5de0d39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs new file mode 100644 index 0000000..39b49db --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs @@ -0,0 +1,710 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Jobs; +using Unity.Burst; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + /// VoxelContour used for recast graphs. + public struct VoxelContour { + public int nverts; + + /// Vertex coordinates, each vertex contains 4 components. + public int vertexStartIndex; + + /// Region ID of the contour + public int reg; + + /// Area ID of the contour. + public int area; + } + + [BurstCompile(CompileSynchronously = true)] + public struct JobBuildContours : IJob { + public CompactVoxelField field; + public float maxError; + public float maxEdgeLength; + public int buildFlags; + public float cellSize; + public NativeList outputContours; + public NativeList outputVerts; + + public void Execute () { + outputContours.Clear(); + outputVerts.Clear(); + + int w = field.width; + int d = field.depth; + int wd = w*d; + + const ushort BorderReg = VoxelUtilityBurst.BorderReg; + + // NOTE: This array may contain uninitialized data, but since we explicitly set all data in it before we use it, it's OK. + var flags = new NativeArray(field.spans.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + // Mark boundaries. (@?) + for (int z = 0; z < wd; z += field.width) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell c = field.cells[x+z]; + + for (int i = (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) { + ushort res = 0; + CompactVoxelSpan s = field.spans[i]; + + if (s.reg == 0 || (s.reg & BorderReg) == BorderReg) { + flags[i] = 0; + continue; + } + + for (int dir = 0; dir < 4; dir++) { + int r = 0; + + if (s.GetConnection(dir) != CompactVoxelField.NotConnected) { + int ni = field.cells[field.GetNeighbourIndex(x+z, dir)].index + s.GetConnection(dir); + r = field.spans[ni].reg; + } + + //@TODO - Why isn't this inside the previous IF + if (r == s.reg) { + res |= (ushort)(1 << dir); + } + } + + //Inverse, mark non connected edges. + flags[i] = (ushort)(res ^ 0xf); + } + } + } + + + NativeList verts = new NativeList(256, Allocator.Temp); + NativeList simplified = new NativeList(64, Allocator.Temp); + + for (int z = 0; z < wd; z += field.width) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell c = field.cells[x+z]; + + for (int i = c.index, ci = c.index+c.count; i < ci; i++) { + if (flags[i] == 0 || flags[i] == 0xf) { + flags[i] = 0; + continue; + } + + int reg = field.spans[i].reg; + + if (reg == 0 || (reg & BorderReg) == BorderReg) { + continue; + } + + int area = field.areaTypes[i]; + + verts.Clear(); + simplified.Clear(); + + WalkContour(x, z, i, flags, verts); + + SimplifyContour(verts, simplified, maxError, buildFlags); + RemoveDegenerateSegments(simplified); + + VoxelContour contour = new VoxelContour { + vertexStartIndex = outputVerts.Length, + nverts = simplified.Length/4, + reg = reg, + area = area, + }; + + outputVerts.AddRange(simplified.AsArray()); + + outputContours.Add(contour); + } + } + } + + verts.Dispose(); + simplified.Dispose(); + + + + // Check and merge droppings. + // Sometimes the previous algorithms can fail and create several outputContours + // per area. This pass will try to merge the holes into the main region. + for (int i = 0; i < outputContours.Length; i++) { + VoxelContour cont = outputContours[i]; + // Check if the contour is would backwards. + var outputVertsArr = outputVerts.AsArray(); + if (CalcAreaOfPolygon2D(outputVertsArr, cont.vertexStartIndex, cont.nverts) < 0) { + // Find another contour which has the same region ID. + int mergeIdx = -1; + for (int j = 0; j < outputContours.Length; j++) { + if (i == j) continue; + if (outputContours[j].nverts > 0 && outputContours[j].reg == cont.reg) { + // Make sure the polygon is correctly oriented. + if (CalcAreaOfPolygon2D(outputVertsArr, outputContours[j].vertexStartIndex, outputContours[j].nverts) > 0) { + mergeIdx = j; + break; + } + } + } + if (mergeIdx == -1) { + // Debug.LogError("rcBuildContours: Could not find merge target for bad contour "+i+"."); + } else { + // Debugging + // Debug.LogWarning ("Fixing contour"); + + VoxelContour mcont = outputContours[mergeIdx]; + // Merge by closest points. + GetClosestIndices(outputVertsArr, mcont.vertexStartIndex, mcont.nverts, cont.vertexStartIndex, cont.nverts, out var ia, out var ib); + + if (ia == -1 || ib == -1) { + // Debug.LogWarning("rcBuildContours: Failed to find merge points for "+i+" and "+mergeIdx+"."); + continue; + } + + + if (!MergeContours(outputVerts, ref mcont, ref cont, ia, ib)) { + //Debug.LogWarning("rcBuildContours: Failed to merge contours "+i+" and "+mergeIdx+"."); + continue; + } + + outputContours[mergeIdx] = mcont; + outputContours[i] = cont; + } + } + } + } + + void GetClosestIndices (NativeArray verts, int vertexStartIndexA, int nvertsa, + int vertexStartIndexB, int nvertsb, + out int ia, out int ib) { + int closestDist = 0xfffffff; + + ia = -1; + ib = -1; + for (int i = 0; i < nvertsa; i++) { + //in is a keyword in C#, so I can't use that as a variable name + int in2 = (i+1) % nvertsa; + int ip = (i+nvertsa-1) % nvertsa; + int va = vertexStartIndexA + i*4; + int van = vertexStartIndexA + in2*4; + int vap = vertexStartIndexA + ip*4; + + for (int j = 0; j < nvertsb; ++j) { + int vb = vertexStartIndexB + j*4; + // vb must be "infront" of va. + if (Ileft(verts, vap, va, vb) && Ileft(verts, va, van, vb)) { + int dx = verts[vb+0] - verts[va+0]; + int dz = (verts[vb+2]/field.width) - (verts[va+2]/field.width); + int d = dx*dx + dz*dz; + if (d < closestDist) { + ia = i; + ib = j; + closestDist = d; + } + } + } + } + } + + public static bool MergeContours (NativeList verts, ref VoxelContour ca, ref VoxelContour cb, int ia, int ib) { + // Note: this will essentially leave junk data in the verts array where the contours were previously. + // This shouldn't be a big problem because MergeContours is normally not called for that many contours (usually none). + int nv = 0; + var startIndex = verts.Length; + + // Copy contour A. + for (int i = 0; i <= ca.nverts; i++) { + int src = ca.vertexStartIndex + ((ia+i) % ca.nverts)*4; + verts.Add(verts[src+0]); + verts.Add(verts[src+1]); + verts.Add(verts[src+2]); + verts.Add(verts[src+3]); + nv++; + } + + // Copy contour B + for (int i = 0; i <= cb.nverts; i++) { + int src = cb.vertexStartIndex + ((ib+i) % cb.nverts)*4; + verts.Add(verts[src+0]); + verts.Add(verts[src+1]); + verts.Add(verts[src+2]); + verts.Add(verts[src+3]); + nv++; + } + + ca.vertexStartIndex = startIndex; + ca.nverts = nv; + + cb.vertexStartIndex = 0; + cb.nverts = 0; + + return true; + } + + public void SimplifyContour (NativeList verts, NativeList simplified, float maxError, int buildFlags) { + // Add initial points. + bool hasConnections = false; + + for (int i = 0; i < verts.Length; i += 4) { + if ((verts[i+3] & VoxelUtilityBurst.ContourRegMask) != 0) { + hasConnections = true; + break; + } + } + + if (hasConnections) { + // The contour has some portals to other regions. + // Add a new point to every location where the region changes. + for (int i = 0, ni = verts.Length/4; i < ni; i++) { + int ii = (i+1) % ni; + bool differentRegs = (verts[i*4+3] & VoxelUtilityBurst.ContourRegMask) != (verts[ii*4+3] & VoxelUtilityBurst.ContourRegMask); + bool areaBorders = (verts[i*4+3] & VoxelUtilityBurst.RC_AREA_BORDER) != (verts[ii*4+3] & VoxelUtilityBurst.RC_AREA_BORDER); + + if (differentRegs || areaBorders) { + simplified.Add(verts[i*4+0]); + simplified.Add(verts[i*4+1]); + simplified.Add(verts[i*4+2]); + simplified.Add(i); + } + } + } + + + if (simplified.Length == 0) { + // If there is no connections at all, + // create some initial points for the simplification process. + // Find lower-left and upper-right vertices of the contour. + int llx = verts[0]; + int lly = verts[1]; + int llz = verts[2]; + int lli = 0; + int urx = verts[0]; + int ury = verts[1]; + int urz = verts[2]; + int uri = 0; + + for (int i = 0; i < verts.Length; i += 4) { + int x = verts[i+0]; + int y = verts[i+1]; + int z = verts[i+2]; + if (x < llx || (x == llx && z < llz)) { + llx = x; + lly = y; + llz = z; + lli = i/4; + } + if (x > urx || (x == urx && z > urz)) { + urx = x; + ury = y; + urz = z; + uri = i/4; + } + } + + simplified.Add(llx); + simplified.Add(lly); + simplified.Add(llz); + simplified.Add(lli); + + simplified.Add(urx); + simplified.Add(ury); + simplified.Add(urz); + simplified.Add(uri); + } + + // Add points until all raw points are within + // error tolerance to the simplified shape. + // This uses the Douglas-Peucker algorithm. + int pn = verts.Length/4; + + //Use the max squared error instead + maxError *= maxError; + + for (int i = 0; i < simplified.Length/4;) { + int ii = (i+1) % (simplified.Length/4); + + int ax = simplified[i*4+0]; + int ay = simplified[i*4+1]; + int az = simplified[i*4+2]; + int ai = simplified[i*4+3]; + + int bx = simplified[ii*4+0]; + int by = simplified[ii*4+1]; + int bz = simplified[ii*4+2]; + int bi = simplified[ii*4+3]; + + // Find maximum deviation from the segment. + float maxd = 0; + int maxi = -1; + int ci, cinc, endi; + + // Traverse the segment in lexilogical order so that the + // max deviation is calculated similarly when traversing + // opposite segments. + if (bx > ax || (bx == ax && bz > az)) { + cinc = 1; + ci = (ai+cinc) % pn; + endi = bi; + } else { + cinc = pn-1; + ci = (bi+cinc) % pn; + endi = ai; + Memory.Swap(ref ax, ref bx); + Memory.Swap(ref az, ref bz); + } + + // Tessellate only outer edges or edges between areas. + if ((verts[ci*4+3] & VoxelUtilityBurst.ContourRegMask) == 0 || + (verts[ci*4+3] & VoxelUtilityBurst.RC_AREA_BORDER) == VoxelUtilityBurst.RC_AREA_BORDER) { + while (ci != endi) { + float d2 = VectorMath.SqrDistancePointSegmentApproximate(verts[ci*4+0], verts[ci*4+2]/field.width, ax, az/field.width, bx, bz/field.width); + + if (d2 > maxd) { + maxd = d2; + maxi = ci; + } + ci = (ci+cinc) % pn; + } + } + + // If the max deviation is larger than accepted error, + // add new point, else continue to next segment. + if (maxi != -1 && maxd > maxError) { + // Add space for the new point. + simplified.ResizeUninitialized(simplified.Length + 4); + + // Move all points after this one, to leave space to insert the new point + simplified.AsUnsafeSpan().Move((i+1)*4, (i+2)*4, simplified.Length-(i+2)*4); + + // Add the point. + simplified[(i+1)*4+0] = verts[maxi*4+0]; + simplified[(i+1)*4+1] = verts[maxi*4+1]; + simplified[(i+1)*4+2] = verts[maxi*4+2]; + simplified[(i+1)*4+3] = maxi; + } else { + i++; + } + } + + // Split too long edges + + float maxEdgeLen = maxEdgeLength / cellSize; + + if (maxEdgeLen > 0 && (buildFlags & (VoxelUtilityBurst.RC_CONTOUR_TESS_WALL_EDGES|VoxelUtilityBurst.RC_CONTOUR_TESS_AREA_EDGES|VoxelUtilityBurst.RC_CONTOUR_TESS_TILE_EDGES)) != 0) { + for (int i = 0; i < simplified.Length/4;) { + if (simplified.Length/4 > 200) { + break; + } + + int ii = (i+1) % (simplified.Length/4); + + int ax = simplified[i*4+0]; + int az = simplified[i*4+2]; + int ai = simplified[i*4+3]; + + int bx = simplified[ii*4+0]; + int bz = simplified[ii*4+2]; + int bi = simplified[ii*4+3]; + + // Find maximum deviation from the segment. + int maxi = -1; + int ci = (ai+1) % pn; + + // Tessellate only outer edges or edges between areas. + bool tess = false; + + // Wall edges. + if ((buildFlags & VoxelUtilityBurst.RC_CONTOUR_TESS_WALL_EDGES) != 0 && (verts[ci*4+3] & VoxelUtilityBurst.ContourRegMask) == 0) + tess = true; + + // Edges between areas. + if ((buildFlags & VoxelUtilityBurst.RC_CONTOUR_TESS_AREA_EDGES) != 0 && (verts[ci*4+3] & VoxelUtilityBurst.RC_AREA_BORDER) == VoxelUtilityBurst.RC_AREA_BORDER) + tess = true; + + // Border of tile + if ((buildFlags & VoxelUtilityBurst.RC_CONTOUR_TESS_TILE_EDGES) != 0 && (verts[ci*4+3] & VoxelUtilityBurst.BorderReg) == VoxelUtilityBurst.BorderReg) + tess = true; + + if (tess) { + int dx = bx - ax; + int dz = (bz/field.width) - (az/field.width); + if (dx*dx + dz*dz > maxEdgeLen*maxEdgeLen) { + // Round based on the segments in lexilogical order so that the + // max tesselation is consistent regardles in which direction + // segments are traversed. + int n = bi < ai ? (bi+pn - ai) : (bi - ai); + if (n > 1) { + if (bx > ax || (bx == ax && bz > az)) { + maxi = (ai + n/2) % pn; + } else { + maxi = (ai + (n+1)/2) % pn; + } + } + } + } + + // If the max deviation is larger than accepted error, + // add new point, else continue to next segment. + if (maxi != -1) { + // Add space for the new point. + //simplified.resize(simplified.size()+4); + simplified.Resize(simplified.Length + 4, NativeArrayOptions.UninitializedMemory); + + simplified.AsUnsafeSpan().Move((i+1)*4, (i+2)*4, simplified.Length-(i+2)*4); + + // Add the point. + simplified[(i+1)*4+0] = verts[maxi*4+0]; + simplified[(i+1)*4+1] = verts[maxi*4+1]; + simplified[(i+1)*4+2] = verts[maxi*4+2]; + simplified[(i+1)*4+3] = maxi; + } else { + ++i; + } + } + } + + for (int i = 0; i < simplified.Length/4; i++) { + // The edge vertex flag is take from the current raw point, + // and the neighbour region is take from the next raw point. + int ai = (simplified[i*4+3]+1) % pn; + int bi = simplified[i*4+3]; + simplified[i*4+3] = (verts[ai*4+3] & VoxelUtilityBurst.ContourRegMask) | (verts[bi*4+3] & VoxelUtilityBurst.RC_BORDER_VERTEX); + } + } + + public void WalkContour (int x, int z, int i, NativeArray flags, NativeList verts) { + // Choose the first non-connected edge + int dir = 0; + + while ((flags[i] & (ushort)(1 << dir)) == 0) { + dir++; + } + + int startDir = dir; + int startI = i; + + int area = field.areaTypes[i]; + + int iter = 0; + + while (iter++ < 40000) { + // Are we facing a region edge + if ((flags[i] & (ushort)(1 << dir)) != 0) { + // Choose the edge corner + bool isBorderVertex = false; + bool isAreaBorder = false; + + int px = x; + int py = GetCornerHeight(x, z, i, dir, ref isBorderVertex); + int pz = z; + + // Offset the vertex to land on the corner of the span. + // The resulting coordinates have an implicit 1/2 voxel offset because all corners + // are in the middle between two adjacent integer voxel coordinates. + switch (dir) { + case 0: pz += field.width; break; + case 1: px++; pz += field.width; break; + case 2: px++; break; + } + + int r = 0; + CompactVoxelSpan s = field.spans[i]; + + if (s.GetConnection(dir) != CompactVoxelField.NotConnected) { + int ni = (int)field.cells[field.GetNeighbourIndex(x+z, dir)].index + s.GetConnection(dir); + r = (int)field.spans[ni].reg; + + if (area != field.areaTypes[ni]) { + isAreaBorder = true; + } + } + + if (isBorderVertex) { + r |= VoxelUtilityBurst.RC_BORDER_VERTEX; + } + if (isAreaBorder) { + r |= VoxelUtilityBurst.RC_AREA_BORDER; + } + + verts.Add(px); + verts.Add(py); + verts.Add(pz); + verts.Add(r); + + flags[i] = (ushort)(flags[i] & ~(1 << dir)); // Remove visited edges + + // & 0x3 is the same as % 4 (for positive numbers) + dir = (dir+1) & 0x3; // Rotate CW + } else { + int ni = -1; + int nx = x + VoxelUtilityBurst.DX[dir]; + int nz = z + VoxelUtilityBurst.DZ[dir]*field.width; + + CompactVoxelSpan s = field.spans[i]; + + if (s.GetConnection(dir) != CompactVoxelField.NotConnected) { + CompactVoxelCell nc = field.cells[nx+nz]; + ni = (int)nc.index + s.GetConnection(dir); + } + + if (ni == -1) { + Debug.LogWarning("Degenerate triangles might have been generated.\n" + + "Usually this is not a problem, but if you have a static level, try to modify the graph settings slightly to avoid this edge case."); + return; + } + x = nx; + z = nz; + i = ni; + + // & 0x3 is the same as % 4 (modulo 4) + dir = (dir+3) & 0x3; // Rotate CCW + } + + if (startI == i && startDir == dir) { + break; + } + } + } + + public int GetCornerHeight (int x, int z, int i, int dir, ref bool isBorderVertex) { + CompactVoxelSpan s = field.spans[i]; + + int cornerHeight = (int)s.y; + + // dir + 1 step in the clockwise direction + int dirp = (dir+1) & 0x3; + + unsafe { + // We need a small buffer to hold regions for each axis aligned neighbour. + // This requires unsafe, though. In future C# versions we can use Span. + // + // dir + // X----> + // dirp | + // v + // + // + // The regs array will contain the regions for the following spans, + // where the 0th span is the current span. + // 'x' signifies the position of the corner we are interested in. + // This is the shared vertex corner the four spans. + // It is conceptually at the current span's position + 0.5*dir + 0.5*dirp + // + // + // 0 --------- 1 -> dir + // | | + // | x | + // | | + // 3 --------- 2 + // + // | dirp + // v + // + var regs = stackalloc uint[] { 0, 0, 0, 0 }; + + regs[0] = (uint)field.spans[i].reg | ((uint)field.areaTypes[i] << 16); + + if (s.GetConnection(dir) != CompactVoxelField.NotConnected) { + int neighbourCell = field.GetNeighbourIndex(x+z, dir); + int ni = (int)field.cells[neighbourCell].index + s.GetConnection(dir); + + CompactVoxelSpan ns = field.spans[ni]; + + cornerHeight = System.Math.Max(cornerHeight, (int)ns.y); + regs[1] = (uint)ns.reg | ((uint)field.areaTypes[ni] << 16); + + if (ns.GetConnection(dirp) != CompactVoxelField.NotConnected) { + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, dirp); + int ni2 = (int)field.cells[neighbourCell2].index + ns.GetConnection(dirp); + + CompactVoxelSpan ns2 = field.spans[ni2]; + + cornerHeight = System.Math.Max(cornerHeight, (int)ns2.y); + regs[2] = (uint)ns2.reg | ((uint)field.areaTypes[ni2] << 16); + } + } + + if (s.GetConnection(dirp) != CompactVoxelField.NotConnected) { + int neighbourCell = field.GetNeighbourIndex(x+z, dirp); + int ni = (int)field.cells[neighbourCell].index + s.GetConnection(dirp); + + CompactVoxelSpan ns = field.spans[ni]; + + cornerHeight = System.Math.Max(cornerHeight, (int)ns.y); + regs[3] = (uint)ns.reg | ((uint)field.areaTypes[ni] << 16); + + if (ns.GetConnection(dir) != CompactVoxelField.NotConnected) { + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, dir); + int ni2 = (int)field.cells[neighbourCell2].index + ns.GetConnection(dir); + + CompactVoxelSpan ns2 = field.spans[ni2]; + + cornerHeight = System.Math.Max(cornerHeight, (int)ns2.y); + regs[2] = (uint)ns2.reg | ((uint)field.areaTypes[ni2] << 16); + } + } + + // Zeroes show up when there are no connections to some spans. E.g. if the current span is on a ledge. + bool noZeros = regs[0] != 0 && regs[1] != 0 && regs[2] != 0 && regs[3] != 0; + + // Check if the vertex is special edge vertex, these vertices will be removed later. + for (int j = 0; j < 4; ++j) { + int a = j; + int b = (j+1) & 0x3; + int c = (j+2) & 0x3; + int d = (j+3) & 0x3; + + // The vertex is a border vertex there are two same exterior cells in a row, + // followed by two interior cells and none of the regions are out of bounds. + bool twoSameExts = (regs[a] & regs[b] & VoxelUtilityBurst.BorderReg) != 0 && regs[a] == regs[b]; + bool twoInts = ((regs[c] | regs[d]) & VoxelUtilityBurst.BorderReg) == 0; + bool intsSameArea = (regs[c]>>16) == (regs[d]>>16); + if (twoSameExts && twoInts && intsSameArea && noZeros) { + isBorderVertex = true; + break; + } + } + } + + return cornerHeight; + } + + static void RemoveRange (NativeList arr, int index, int count) { + for (int i = index; i < arr.Length - count; i++) { + arr[i] = arr[i+count]; + } + arr.Resize(arr.Length - count, NativeArrayOptions.UninitializedMemory); + } + + static void RemoveDegenerateSegments (NativeList simplified) { + // Remove adjacent vertices which are equal on xz-plane, + // or else the triangulator will get confused + for (int i = 0; i < simplified.Length/4; i++) { + int ni = i+1; + if (ni >= (simplified.Length/4)) + ni = 0; + + if (simplified[i*4+0] == simplified[ni*4+0] && + simplified[i*4+2] == simplified[ni*4+2]) { + // Degenerate segment, remove. + RemoveRange(simplified, i, 4); + } + } + } + + int CalcAreaOfPolygon2D (NativeArray verts, int vertexStartIndex, int nverts) { + int area = 0; + + for (int i = 0, j = nverts-1; i < nverts; j = i++) { + int vi = vertexStartIndex + i*4; + int vj = vertexStartIndex + j*4; + area += verts[vi+0] * (verts[vj+2]/field.width) - verts[vj+0] * (verts[vi+2]/field.width); + } + + return (area+1) / 2; + } + + static bool Ileft (NativeArray verts, int a, int b, int c) { + return (verts[b+0] - verts[a+0]) * (verts[c+2] - verts[a+2]) - (verts[c+0] - verts[a+0]) * (verts[b+2] - verts[a+2]) <= 0; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs.meta new file mode 100644 index 0000000..712ca53 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelContour.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: aba1f429a9dee0ef98d35221ff450cda +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs new file mode 100644 index 0000000..b236330 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs @@ -0,0 +1,542 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Jobs; +using Unity.Burst; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + using System; + using Pathfinding.Jobs; + using Pathfinding.Util; +#if MODULE_COLLECTIONS_2_1_0_OR_NEWER + using NativeHashMapInt3Int = Unity.Collections.NativeHashMap; +#else + using NativeHashMapInt3Int = Unity.Collections.NativeParallelHashMap; +#endif + + /// VoxelMesh used for recast graphs. + public struct VoxelMesh : IArenaDisposable { + /// Vertices of the mesh + public NativeList verts; + + /// + /// Triangles of the mesh. + /// Each element points to a vertex in the array + /// + public NativeList tris; + + /// Area index for each triangle + public NativeList areas; + + void IArenaDisposable.DisposeWith (DisposeArena arena) { + arena.Add(verts); + arena.Add(tris); + arena.Add(areas); + } + } + + /// Builds a polygon mesh from a contour set. + [BurstCompile] + public struct JobBuildMesh : IJob { + public NativeList contourVertices; + /// contour set to build a mesh from. + public NativeList contours; + /// Results will be written to this mesh. + public VoxelMesh mesh; + public CompactVoxelField field; + + /// + /// Returns T iff (v_i, v_j) is a proper internal + /// diagonal of P. + /// + static bool Diagonal (int i, int j, int n, NativeArray verts, NativeArray indices) { + return InCone(i, j, n, verts, indices) && Diagonalie(i, j, n, verts, indices); + } + + static bool InCone (int i, int j, int n, NativeArray verts, NativeArray indices) { + int pi = (indices[i] & 0x0fffffff) * 3; + int pj = (indices[j] & 0x0fffffff) * 3; + int pi1 = (indices[Next(i, n)] & 0x0fffffff) * 3; + int pin1 = (indices[Prev(i, n)] & 0x0fffffff) * 3; + + // If P[i] is a convex vertex [ i+1 left or on (i-1,i) ]. + if (LeftOn(pin1, pi, pi1, verts)) + return Left(pi, pj, pin1, verts) && Left(pj, pi, pi1, verts); + // Assume (i-1,i,i+1) not collinear. + // else P[i] is reflex. + return !(LeftOn(pi, pj, pi1, verts) && LeftOn(pj, pi, pin1, verts)); + } + + /// + /// Returns true iff c is strictly to the left of the directed + /// line through a to b. + /// + static bool Left (int a, int b, int c, NativeArray verts) { + return Area2(a, b, c, verts) < 0; + } + + static bool LeftOn (int a, int b, int c, NativeArray verts) { + return Area2(a, b, c, verts) <= 0; + } + + static bool Collinear (int a, int b, int c, NativeArray verts) { + return Area2(a, b, c, verts) == 0; + } + + public static int Area2 (int a, int b, int c, NativeArray verts) { + return (verts[b] - verts[a]) * (verts[c+2] - verts[a+2]) - (verts[c+0] - verts[a+0]) * (verts[b+2] - verts[a+2]); + } + + /// + /// Returns T iff (v_i, v_j) is a proper internal *or* external + /// diagonal of P, *ignoring edges incident to v_i and v_j*. + /// + static bool Diagonalie (int i, int j, int n, NativeArray verts, NativeArray indices) { + int d0 = (indices[i] & 0x0fffffff) * 3; + int d1 = (indices[j] & 0x0fffffff) * 3; + + /*int a = (i+1) % indices.Length; + * if (a == j) a = (i-1 + indices.Length) % indices.Length; + * int a_v = (indices[a] & 0x0fffffff) * 4; + * + * if (a != j && Collinear (d0,a_v,d1,verts)) { + * return false; + * }*/ + + // For each edge (k,k+1) of P + for (int k = 0; k < n; k++) { + int k1 = Next(k, n); + // Skip edges incident to i or j + if (!((k == i) || (k1 == i) || (k == j) || (k1 == j))) { + int p0 = (indices[k] & 0x0fffffff) * 3; + int p1 = (indices[k1] & 0x0fffffff) * 3; + + if (Vequal(d0, p0, verts) || Vequal(d1, p0, verts) || Vequal(d0, p1, verts) || Vequal(d1, p1, verts)) + continue; + + if (Intersect(d0, d1, p0, p1, verts)) + return false; + } + } + + + return true; + } + + // Exclusive or: true iff exactly one argument is true. + // The arguments are negated to ensure that they are 0/1 + // values. Then the bitwise Xor operator may apply. + // (This idea is due to Michael Baldwin.) + static bool Xorb (bool x, bool y) { + return !x ^ !y; + } + + // Returns true iff ab properly intersects cd: they share + // a point interior to both segments. The properness of the + // intersection is ensured by using strict leftness. + static bool IntersectProp (int a, int b, int c, int d, NativeArray verts) { + // Eliminate improper cases. + if (Collinear(a, b, c, verts) || Collinear(a, b, d, verts) || + Collinear(c, d, a, verts) || Collinear(c, d, b, verts)) + return false; + + return Xorb(Left(a, b, c, verts), Left(a, b, d, verts)) && Xorb(Left(c, d, a, verts), Left(c, d, b, verts)); + } + + // Returns T iff (a,b,c) are collinear and point c lies + // on the closed segement ab. + static bool Between (int a, int b, int c, NativeArray verts) { + if (!Collinear(a, b, c, verts)) + return false; + // If ab not vertical, check betweenness on x; else on y. + if (verts[a+0] != verts[b+0]) + return ((verts[a+0] <= verts[c+0]) && (verts[c+0] <= verts[b+0])) || ((verts[a+0] >= verts[c+0]) && (verts[c+0] >= verts[b+0])); + else + return ((verts[a+2] <= verts[c+2]) && (verts[c+2] <= verts[b+2])) || ((verts[a+2] >= verts[c+2]) && (verts[c+2] >= verts[b+2])); + } + + // Returns true iff segments ab and cd intersect, properly or improperly. + static bool Intersect (int a, int b, int c, int d, NativeArray verts) { + if (IntersectProp(a, b, c, d, verts)) + return true; + else if (Between(a, b, c, verts) || Between(a, b, d, verts) || + Between(c, d, a, verts) || Between(c, d, b, verts)) + return true; + else + return false; + } + + static bool Vequal (int a, int b, NativeArray verts) { + return verts[a+0] == verts[b+0] && verts[a+2] == verts[b+2]; + } + + /// (i-1+n) % n assuming 0 <= i < n + static int Prev (int i, int n) { return i-1 >= 0 ? i-1 : n-1; } + /// (i+1) % n assuming 0 <= i < n + static int Next (int i, int n) { return i+1 < n ? i+1 : 0; } + + static int AddVertex (NativeList vertices, NativeHashMapInt3Int vertexMap, Int3 vertex) { + if (vertexMap.TryGetValue(vertex, out var index)) { + return index; + } + vertices.AddNoResize(vertex); + vertexMap.Add(vertex, vertices.Length-1); + return vertices.Length-1; + } + + public void Execute () { + // Maximum allowed vertices per polygon. Currently locked to 3. + var nvp = 3; + + int maxVertices = 0; + int maxTris = 0; + int maxVertsPerCont = 0; + + for (int i = 0; i < contours.Length; i++) { + // Skip null contours. + if (contours[i].nverts < 3) continue; + + maxVertices += contours[i].nverts; + maxTris += contours[i].nverts - 2; + maxVertsPerCont = System.Math.Max(maxVertsPerCont, contours[i].nverts); + } + + mesh.verts.Clear(); + if (maxVertices > mesh.verts.Capacity) mesh.verts.SetCapacity(maxVertices); + mesh.tris.ResizeUninitialized(maxTris*nvp); + mesh.areas.ResizeUninitialized(maxTris); + var verts = mesh.verts; + var polys = mesh.tris; + var areas = mesh.areas; + + var indices = new NativeArray(maxVertsPerCont, Allocator.Temp); + var tris = new NativeArray(maxVertsPerCont*3, Allocator.Temp); + var verticesToRemove = new NativeArray(maxVertices, Allocator.Temp); + var vertexPointers = new NativeHashMapInt3Int(maxVertices, Allocator.Temp); + + int polyIndex = 0; + int areaIndex = 0; + + for (int i = 0; i < contours.Length; i++) { + VoxelContour cont = contours[i]; + + // Skip degenerate contours + if (cont.nverts < 3) { + continue; + } + + for (int j = 0; j < cont.nverts; j++) { + // Convert the z coordinate from the form z*voxelArea.width which is used in other places for performance + contourVertices[cont.vertexStartIndex + j*4+2] /= field.width; + } + + // Copy the vertex positions + for (int j = 0; j < cont.nverts; j++) { + // Try to remove all border vertices + // See https://digestingduck.blogspot.com/2009/08/navmesh-height-accuracy-pt-5.html + var vertexRegion = contourVertices[cont.vertexStartIndex + j*4+3]; + + // Add a new vertex, or reuse an existing one if it has already been added to the mesh + var idx = AddVertex(verts, vertexPointers, new Int3( + contourVertices[cont.vertexStartIndex + j*4], + contourVertices[cont.vertexStartIndex + j*4+1], + contourVertices[cont.vertexStartIndex + j*4+2] + )); + indices[j] = idx; + verticesToRemove[idx] = (vertexRegion & VoxelUtilityBurst.RC_BORDER_VERTEX) != 0; + } + + // Triangulate the contour + int ntris = Triangulate(cont.nverts, verts.AsArray().Reinterpret(12), indices, tris); + + if (ntris < 0) { + // Degenerate triangles. This may lead to a hole in the navmesh. + // We add the triangles that the triangulation generated before it failed. + ntris = -ntris; + } + + // Copy the resulting triangles to the mesh + for (int j = 0; j < ntris*3; polyIndex++, j++) { + polys[polyIndex] = tris[j]; + } + + // Mark all triangles generated by this contour + // as having the area cont.area + for (int j = 0; j < ntris; areaIndex++, j++) { + areas[areaIndex] = cont.area; + } + } + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (areaIndex > mesh.areas.Length) throw new System.Exception("Ended up at an unexpected area index"); + if (polyIndex > mesh.tris.Length) throw new System.Exception("Ended up at an unexpected poly index"); +#endif + + // polyIndex might in rare cases not be equal to mesh.tris.Length. + // This can happen if degenerate triangles were generated. + // So we make sure the list is truncated to the right size here. + mesh.tris.ResizeUninitialized(polyIndex); + // Same thing for area index + mesh.areas.ResizeUninitialized(areaIndex); + + RemoveTileBorderVertices(ref mesh, verticesToRemove); + } + + void RemoveTileBorderVertices (ref VoxelMesh mesh, NativeArray verticesToRemove) { + // Iterate in reverse to avoid having to update the verticesToRemove array as we remove vertices + var vertexScratch = new NativeArray(mesh.verts.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int i = mesh.verts.Length - 1; i >= 0; i--) { + if (verticesToRemove[i] && CanRemoveVertex(ref mesh, i, vertexScratch.AsUnsafeSpan())) { + RemoveVertex(ref mesh, i); + } + } + } + + bool CanRemoveVertex (ref VoxelMesh mesh, int vertexToRemove, UnsafeSpan vertexScratch) { + UnityEngine.Assertions.Assert.IsTrue(vertexScratch.Length >= mesh.verts.Length); + + int remainingEdges = 0; + for (int i = 0; i < mesh.tris.Length; i += 3) { + int touched = 0; + for (int j = 0; j < 3; j++) { + if (mesh.tris[i+j] == vertexToRemove) { + // This vertex is used by a triangle + touched++; + } + } + + if (touched > 0) { + if (touched > 1) throw new Exception("Degenerate triangle. This should have already been removed."); + // If one vertex is removed from a triangle, 1 edge remains + remainingEdges++; + } + } + + if (remainingEdges <= 2) { + // There would be too few edges remaining to create a polygon. + // This can happen for example when a tip of a triangle is marked + // as deletion, but there are no other polys that share the vertex. + // In this case, the vertex should not be removed. + return false; + } + + vertexScratch.FillZeros(); + + for (int i = 0; i < mesh.tris.Length; i += 3) { + for (int a = 0, b = 2; a < 3; b = a++) { + if (mesh.tris[i+a] == vertexToRemove || mesh.tris[i+b] == vertexToRemove) { + // This edge is used by a triangle + int v1 = mesh.tris[i+a]; + int v2 = mesh.tris[i+b]; + + // Update the shared count for the edge. + // We identify the edge by the vertex index which is not the vertex to remove. + vertexScratch[v2 == vertexToRemove ? v1 : v2]++; + } + } + } + + int openEdges = 0; + for (int i = 0; i < vertexScratch.Length; i++) { + if (vertexScratch[i] == 1) openEdges++; + } + + // There should be no more than 2 open edges. + // This catches the case that two non-adjacent polygons + // share the removed vertex. In that case, do not remove the vertex. + return openEdges <= 2; + } + + void RemoveVertex (ref VoxelMesh mesh, int vertexToRemove) { + // Note: Assumes CanRemoveVertex has been called and returned true + + var remainingEdges = new NativeList(16, Allocator.Temp); + var area = -1; + // Find all triangles that use this vertex + for (int i = 0; i < mesh.tris.Length; i += 3) { + int touched = -1; + for (int j = 0; j < 3; j++) { + if (mesh.tris[i+j] == vertexToRemove) { + // This vertex is used by a triangle + touched = j; + break; + } + } + if (touched != -1) { + // Note: Only vertices that are not on an area border will be chosen (see GetCornerHeight), + // so it is safe to assume that all triangles that share this vertex also share an area. + area = mesh.areas[i/3]; + // If one vertex is removed from a triangle, 1 edge remains + remainingEdges.Add(mesh.tris[i+((touched+1) % 3)]); + remainingEdges.Add(mesh.tris[i+((touched+2) % 3)]); + + mesh.tris[i+0] = mesh.tris[mesh.tris.Length-3+0]; + mesh.tris[i+1] = mesh.tris[mesh.tris.Length-3+1]; + mesh.tris[i+2] = mesh.tris[mesh.tris.Length-3+2]; + + mesh.tris.Length -= 3; + mesh.areas.RemoveAtSwapBack(i/3); + i -= 3; + } + } + + UnityEngine.Assertions.Assert.AreNotEqual(-1, area); + + // Build a sorted list of all vertices in the contour for the hole + var sortedVertices = new NativeList(remainingEdges.Length/2 + 1, Allocator.Temp); + sortedVertices.Add(remainingEdges[remainingEdges.Length-2]); + sortedVertices.Add(remainingEdges[remainingEdges.Length-1]); + remainingEdges.Length -= 2; + + while (remainingEdges.Length > 0) { + for (int i = remainingEdges.Length - 2; i >= 0; i -= 2) { + var a = remainingEdges[i]; + var b = remainingEdges[i+1]; + bool added = false; + if (sortedVertices[0] == b) { + sortedVertices.InsertRange(0, 1); + sortedVertices[0] = a; + added = true; + } + if (sortedVertices[sortedVertices.Length-1] == a) { + sortedVertices.AddNoResize(b); + added = true; + } + if (added) { + // Remove the edge and swap with the last one + remainingEdges[i] = remainingEdges[remainingEdges.Length-2]; + remainingEdges[i+1] = remainingEdges[remainingEdges.Length-1]; + remainingEdges.Length -= 2; + } + } + } + + // Remove the vertex + mesh.verts.RemoveAt(vertexToRemove); + + // Patch indices to account for the removed vertex + for (int i = 0; i < mesh.tris.Length; i++) { + if (mesh.tris[i] > vertexToRemove) mesh.tris[i]--; + } + for (int i = 0; i < sortedVertices.Length; i++) { + if (sortedVertices[i] > vertexToRemove) sortedVertices[i]--; + } + + var maxIndices = (sortedVertices.Length - 2) * 3; + var trisBeforeResize = mesh.tris.Length; + mesh.tris.Length += maxIndices; + int newTriCount = Triangulate( + sortedVertices.Length, + mesh.verts.AsArray().Reinterpret(12), + sortedVertices.AsArray(), + // Insert the new triangles at the end of the array + mesh.tris.AsArray().GetSubArray(trisBeforeResize, maxIndices) + ); + + if (newTriCount < 0) { + // Degenerate triangles. This may lead to a hole in the navmesh. + // We add the triangles that the triangulation generated before it failed. + newTriCount = -newTriCount; + } + + // Resize the triangle array to the correct size + mesh.tris.ResizeUninitialized(trisBeforeResize + newTriCount*3); + mesh.areas.AddReplicate(area, newTriCount); + + UnityEngine.Assertions.Assert.AreEqual(mesh.areas.Length, mesh.tris.Length/3); + } + + static int Triangulate (int n, NativeArray verts, NativeArray indices, NativeArray tris) { + int ntris = 0; + var dst = tris; + int dstIndex = 0; + + // The last bit of the index is used to indicate if the vertex can be removed + // in an ear-cutting operation. + const int CanBeRemovedBit = 0x40000000; + // Used to get only the index value, without any flag bits. + const int IndexMask = 0x0fffffff; + + for (int i = 0; i < n; i++) { + int i1 = Next(i, n); + int i2 = Next(i1, n); + if (Diagonal(i, i2, n, verts, indices)) { + indices[i1] |= CanBeRemovedBit; + } + } + + while (n > 3) { + int minLen = int.MaxValue; + int mini = -1; + + for (int q = 0; q < n; q++) { + int q1 = Next(q, n); + if ((indices[q1] & CanBeRemovedBit) != 0) { + int p0 = (indices[q] & IndexMask) * 3; + int p2 = (indices[Next(q1, n)] & IndexMask) * 3; + + int dx = verts[p2+0] - verts[p0+0]; + int dz = verts[p2+2] - verts[p0+2]; + + + //Squared distance + int len = dx*dx + dz*dz; + + if (len < minLen) { + minLen = len; + mini = q; + } + } + } + + if (mini == -1) { + Debug.LogWarning("Degenerate triangles might have been generated.\n" + + "Usually this is not a problem, but if you have a static level, try to modify the graph settings slightly to avoid this edge case."); + return -ntris; + } + + int i = mini; + int i1 = Next(i, n); + int i2 = Next(i1, n); + + + dst[dstIndex] = indices[i] & IndexMask; + dstIndex++; + dst[dstIndex] = indices[i1] & IndexMask; + dstIndex++; + dst[dstIndex] = indices[i2] & IndexMask; + dstIndex++; + ntris++; + + // Removes P[i1] by copying P[i+1]...P[n-1] left one index. + n--; + for (int k = i1; k < n; k++) { + indices[k] = indices[k+1]; + } + + if (i1 >= n) i1 = 0; + i = Prev(i1, n); + // Update diagonal flags. + if (Diagonal(Prev(i, n), i1, n, verts, indices)) { + indices[i] |= CanBeRemovedBit; + } else { + indices[i] &= IndexMask; + } + if (Diagonal(i, Next(i1, n), n, verts, indices)) { + indices[i1] |= CanBeRemovedBit; + } else { + indices[i1] &= IndexMask; + } + } + + dst[dstIndex] = indices[0] & IndexMask; + dstIndex++; + dst[dstIndex] = indices[1] & IndexMask; + dstIndex++; + dst[dstIndex] = indices[2] & IndexMask; + dstIndex++; + ntris++; + + return ntris; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs.meta new file mode 100644 index 0000000..eb45f18 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelMesh.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 73110b746664b5ec197eda5f732356a5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs new file mode 100644 index 0000000..2576e6e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs @@ -0,0 +1,205 @@ +using Unity.Burst; + +namespace Pathfinding.Graphs.Navmesh.Voxelization { + /// Utility for clipping polygons + internal struct Int3PolygonClipper { + /// Cache this buffer to avoid unnecessary allocations + float[] clipPolygonCache; + + /// Cache this buffer to avoid unnecessary allocations + int[] clipPolygonIntCache; + + /// Initialize buffers if they are null + public void Init () { + if (clipPolygonCache == null) { + clipPolygonCache = new float[7*3]; + clipPolygonIntCache = new int[7*3]; + } + } + + /// + /// Clips a polygon against an axis aligned half plane. + /// + /// Returns: Number of output vertices + /// + /// The vertices will be scaled and then offset, after that they will be cut using either the + /// x axis, y axis or the z axis as the cutting line. The resulting vertices will be added to the + /// vOut array in their original space (i.e before scaling and offsetting). + /// + /// Input vertices + /// Number of input vertices (may be less than the length of the vIn array) + /// Output vertices, needs to be large enough + /// Scale factor for the input vertices + /// Offset to move the input vertices with before cutting + /// Axis to cut along, either x=0, y=1, z=2 + public int ClipPolygon (Int3[] vIn, int n, Int3[] vOut, int multi, int offset, int axis) { + Init(); + int[] d = clipPolygonIntCache; + + for (int i = 0; i < n; i++) { + d[i] = multi*vIn[i][axis]+offset; + } + + // Number of resulting vertices + int m = 0; + + for (int i = 0, j = n-1; i < n; j = i, i++) { + bool prev = d[j] >= 0; + bool curr = d[i] >= 0; + + if (prev != curr) { + double s = (double)d[j] / (d[j] - d[i]); + + vOut[m] = vIn[j] + (vIn[i]-vIn[j])*s; + m++; + } + + if (curr) { + vOut[m] = vIn[i]; + m++; + } + } + + return m; + } + } + + /// Utility for clipping polygons + internal struct VoxelPolygonClipper { + public unsafe fixed float x[8]; + public unsafe fixed float y[8]; + public unsafe fixed float z[8]; + public int n; + + public UnityEngine.Vector3 this[int i] { + set { + unsafe { + x[i] = value.x; + y[i] = value.y; + z[i] = value.z; + } + } + } + + /// + /// Clips a polygon against an axis aligned half plane. + /// The polygons stored in this object are clipped against the half plane at x = -offset. + /// + /// Ouput vertices + /// Scale factor for the input vertices. Should be +1 or -1. If -1 the negative half plane is kept. + /// Offset to move the input vertices with before cutting + public void ClipPolygonAlongX ([NoAlias] ref VoxelPolygonClipper result, float multi, float offset) { + unsafe { + // Number of resulting vertices + int m = 0; + + float dj = multi*x[(n-1)]+offset; + + for (int i = 0, j = n-1; i < n; j = i, i++) { + float di = multi*x[i]+offset; + bool prev = dj >= 0; + bool curr = di >= 0; + + if (prev != curr) { + float s = dj / (dj - di); + result.x[m] = x[j] + (x[i]-x[j])*s; + result.y[m] = y[j] + (y[i]-y[j])*s; + result.z[m] = z[j] + (z[i]-z[j])*s; + m++; + } + + if (curr) { + result.x[m] = x[i]; + result.y[m] = y[i]; + result.z[m] = z[i]; + m++; + } + + dj = di; + } + + result.n = m; + } + } + + /// + /// Clips a polygon against an axis aligned half plane. + /// The polygons stored in this object are clipped against the half plane at z = -offset. + /// + /// Ouput vertices. Only the Y and Z coordinates are calculated. The X coordinates are undefined. + /// Scale factor for the input vertices. Should be +1 or -1. If -1 the negative half plane is kept. + /// Offset to move the input vertices with before cutting + public void ClipPolygonAlongZWithYZ ([NoAlias] ref VoxelPolygonClipper result, float multi, float offset) { + unsafe { + // Number of resulting vertices + int m = 0; + + Unity.Burst.CompilerServices.Hint.Assume(n >= 0); + Unity.Burst.CompilerServices.Hint.Assume(n <= 8); + float dj = multi*z[(n-1)]+offset; + + for (int i = 0, j = n-1; i < n; j = i, i++) { + float di = multi*z[i]+offset; + bool prev = dj >= 0; + bool curr = di >= 0; + + if (prev != curr) { + float s = dj / (dj - di); + result.y[m] = y[j] + (y[i]-y[j])*s; + result.z[m] = z[j] + (z[i]-z[j])*s; + m++; + } + + if (curr) { + result.y[m] = y[i]; + result.z[m] = z[i]; + m++; + } + + dj = di; + } + + result.n = m; + } + } + + /// + /// Clips a polygon against an axis aligned half plane. + /// The polygons stored in this object are clipped against the half plane at z = -offset. + /// + /// Ouput vertices. Only the Y coordinates are calculated. The X and Z coordinates are undefined. + /// Scale factor for the input vertices. Should be +1 or -1. If -1 the negative half plane is kept. + /// Offset to move the input vertices with before cutting + public void ClipPolygonAlongZWithY ([NoAlias] ref VoxelPolygonClipper result, float multi, float offset) { + unsafe { + // Number of resulting vertices + int m = 0; + + Unity.Burst.CompilerServices.Hint.Assume(n >= 3); + Unity.Burst.CompilerServices.Hint.Assume(n <= 8); + float dj = multi*z[n-1]+offset; + + for (int i = 0, j = n-1; i < n; j = i, i++) { + float di = multi*z[i]+offset; + bool prev = dj >= 0; + bool curr = di >= 0; + + if (prev != curr) { + float s = dj / (dj - di); + result.y[m] = y[j] + (y[i]-y[j])*s; + m++; + } + + if (curr) { + result.y[m] = y[i]; + m++; + } + + dj = di; + } + + result.n = m; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs.meta new file mode 100644 index 0000000..6ab0fa5 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelPolygonClipper.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 10347e1eaceee428fa14386ccbaffde5 +timeCreated: 1454161567 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs new file mode 100644 index 0000000..a99fddc --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs @@ -0,0 +1,484 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Mathematics; +using Unity.Jobs; +using Unity.Burst; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + using Pathfinding.Util; + using Unity.Collections.LowLevel.Unsafe; + + public struct RasterizationMesh { + public UnsafeSpan vertices; + + public UnsafeSpan triangles; + + public int area; + + /// World bounds of the mesh. Assumed to already be multiplied with the matrix + public Bounds bounds; + + public Matrix4x4 matrix; + + /// + /// If true then the mesh will be treated as solid and its interior will be unwalkable. + /// The unwalkable region will be the minimum to maximum y coordinate in each cell. + /// + public bool solid; + + /// If true, both sides of the mesh will be walkable. If false, only the side that the normal points towards will be walkable + public bool doubleSided; + + /// If true, the will be interpreted as a node tag and applied to the final nodes + public bool areaIsTag; + + /// + /// If true, the mesh will be flattened to the base of the graph during rasterization. + /// + /// This is intended for rasterizing 2D meshes which always lie in a single plane. + /// + /// This will also cause unwalkable spans have precedence over walkable ones at all times, instead of + /// only when the unwalkable span is sufficiently high up over a walkable span. Since when flattening, + /// "sufficiently high up" makes no sense. + /// + public bool flatten; + } + + [BurstCompile(CompileSynchronously = true)] + public struct JobVoxelize : IJob { + [ReadOnly] + public NativeArray inputMeshes; + + [ReadOnly] + public NativeArray bucket; + + /// Maximum ledge height that is considered to still be traversable. [Limit: >=0] [Units: vx] + public int voxelWalkableClimb; + + /// + /// Minimum floor to 'ceiling' height that will still allow the floor area to + /// be considered walkable. [Limit: >= 3] [Units: vx] + /// + public uint voxelWalkableHeight; + + /// The xz-plane cell size to use for fields. [Limit: > 0] [Units: wu] + public float cellSize; + + /// The y-axis cell size to use for fields. [Limit: > 0] [Units: wu] + public float cellHeight; + + /// The maximum slope that is considered walkable. [Limits: 0 <= value < 90] [Units: Degrees] + public float maxSlope; + + public Matrix4x4 graphTransform; + public Bounds graphSpaceBounds; + public Vector2 graphSpaceLimits; + public LinkedVoxelField voxelArea; + + public void Execute () { + // Transform from voxel space to graph space. + // then scale from voxel space (one unit equals one voxel) + // Finally add min + Matrix4x4 voxelMatrix = Matrix4x4.TRS(graphSpaceBounds.min, Quaternion.identity, Vector3.one) * Matrix4x4.Scale(new Vector3(cellSize, cellHeight, cellSize)); + + // Transform from voxel space to world space + // add half a voxel to fix rounding + var transform = graphTransform * voxelMatrix * Matrix4x4.Translate(new Vector3(0.5f, 0, 0.5f)); + var world2voxelMatrix = transform.inverse; + + // Cosine of the slope limit in voxel space (some tweaks are needed because the voxel space might be stretched out along the y axis) + float slopeLimit = math.cos(math.atan((cellSize/cellHeight)*math.tan(maxSlope*Mathf.Deg2Rad))); + + // Temporary arrays used for rasterization + var clipperOrig = new VoxelPolygonClipper(); + var clipperX1 = new VoxelPolygonClipper(); + var clipperX2 = new VoxelPolygonClipper(); + var clipperZ1 = new VoxelPolygonClipper(); + var clipperZ2 = new VoxelPolygonClipper(); + + // Find the largest lengths of vertex arrays and check for meshes which can be skipped + int maxVerts = 0; + for (int m = 0; m < bucket.Length; m++) { + maxVerts = math.max(inputMeshes[bucket[m]].vertices.Length, maxVerts); + } + + // Create buffer, here vertices will be stored multiplied with the local-to-voxel-space matrix + var verts = new NativeArray(maxVerts, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + int width = voxelArea.width; + int depth = voxelArea.depth; + + // These will be width-1 and depth-1 respectively for all but the last tile row and column of the graph + var cropX = Mathf.Min(width - 1, Mathf.CeilToInt((graphSpaceLimits.x - graphSpaceBounds.min.x) / cellSize)); + var cropZ = Mathf.Min(depth - 1, Mathf.CeilToInt((graphSpaceLimits.y - graphSpaceBounds.min.z) / cellSize)); + + // This loop is the hottest place in the whole rasterization process + // it usually accounts for around 50% of the time + for (int m = 0; m < bucket.Length; m++) { + RasterizationMesh mesh = inputMeshes[bucket[m]]; + var meshMatrix = mesh.matrix; + + // Flip the orientation of all faces if the mesh is scaled in such a way + // that the face orientations would change + // This happens for example if a mesh has a negative scale along an odd number of axes + // e.g it happens for the scale (-1, 1, 1) but not for (-1, -1, 1) or (1,1,1) + var flipOrientation = VectorMath.ReversesFaceOrientations(meshMatrix); + + var vs = mesh.vertices; + var tris = mesh.triangles; + + // Transform vertices first to world space and then to voxel space + var localToVoxelMatrix = (float4x4)(world2voxelMatrix * mesh.matrix); + for (int i = 0; i < vs.Length; i++) verts[i] = math.transform(localToVoxelMatrix, vs[i]); + + int mesharea = mesh.area; + if (mesh.areaIsTag) { + mesharea |= VoxelUtilityBurst.TagReg; + } + + var meshBounds = new IntRect(); + + for (int i = 0; i < tris.Length; i += 3) { + float3 p1 = verts[tris[i]]; + float3 p2 = verts[tris[i+1]]; + float3 p3 = verts[tris[i+2]]; + + if (flipOrientation) { + var tmp = p1; + p1 = p3; + p3 = tmp; + } + + int minX = (int)math.min(math.min(p1.x, p2.x), p3.x); + int minZ = (int)math.min(math.min(p1.z, p2.z), p3.z); + + int maxX = (int)math.ceil(math.max(math.max(p1.x, p2.x), p3.x)); + int maxZ = (int)math.ceil(math.max(math.max(p1.z, p2.z), p3.z)); + + // Check if the mesh is completely out of bounds + if (minX > cropX || minZ > cropZ || maxX < 0 || maxZ < 0) continue; + + minX = math.clamp(minX, 0, cropX); + maxX = math.clamp(maxX, 0, cropX); + minZ = math.clamp(minZ, 0, cropZ); + maxZ = math.clamp(maxZ, cropZ, cropZ); + + if (i == 0) meshBounds = new IntRect(minX, minZ, minX, minZ); + meshBounds.xmin = math.min(meshBounds.xmin, minX); + meshBounds.xmax = math.max(meshBounds.xmax, maxX); + meshBounds.ymin = math.min(meshBounds.ymin, minZ); + meshBounds.ymax = math.max(meshBounds.ymax, maxZ); + + // Check max slope + float3 normal = math.cross(p2-p1, p3-p1); + float cosSlopeAngle = math.normalizesafe(normal).y; + if (mesh.doubleSided) cosSlopeAngle = math.abs(cosSlopeAngle); + int area = cosSlopeAngle < slopeLimit ? CompactVoxelField.UnwalkableArea : 1 + mesharea; + + clipperOrig[0] = p1; + clipperOrig[1] = p2; + clipperOrig[2] = p3; + clipperOrig.n = 3; + + for (int x = minX; x <= maxX; x++) { + clipperOrig.ClipPolygonAlongX(ref clipperX1, 1f, -x+0.5f); + + if (clipperX1.n < 3) { + continue; + } + + clipperX1.ClipPolygonAlongX(ref clipperX2, -1F, x+0.5F); + + if (clipperX2.n < 3) { + continue; + } + + float clampZ1, clampZ2; + unsafe { + clampZ1 = clampZ2 = clipperX2.z[0]; + for (int q = 1; q < clipperX2.n; q++) { + float val = clipperX2.z[q]; + clampZ1 = math.min(clampZ1, val); + clampZ2 = math.max(clampZ2, val); + } + } + + int clampZ1I = math.clamp((int)math.round(clampZ1), 0, cropX); + int clampZ2I = math.clamp((int)math.round(clampZ2), 0, cropZ); + + for (int z = clampZ1I; z <= clampZ2I; z++) { + clipperX2.ClipPolygonAlongZWithYZ(ref clipperZ1, 1F, -z+0.5F); + + if (clipperZ1.n < 3) { + continue; + } + + clipperZ1.ClipPolygonAlongZWithY(ref clipperZ2, -1F, z+0.5F); + if (clipperZ2.n < 3) { + continue; + } + + + if (mesh.flatten) { + voxelArea.AddFlattenedSpan(z*width+x, area); + } else { + float sMin, sMax; + unsafe { + var u = clipperZ2.y[0]; + sMin = sMax = u; + for (int q = 1; q < clipperZ2.n; q++) { + float val = clipperZ2.y[q]; + sMin = math.min(sMin, val); + sMax = math.max(sMax, val); + } + } + + int maxi = (int)math.ceil(sMax); + // Make sure mini >= 0 + int mini = (int)sMin; + // Make sure the span is at least 1 voxel high + maxi = math.max(mini+1, maxi); + + voxelArea.AddLinkedSpan(z*width+x, mini, maxi, area, voxelWalkableClimb, m); + } + } + } + } + + if (mesh.solid) { + for (int z = meshBounds.ymin; z <= meshBounds.ymax; z++) { + for (int x = meshBounds.xmin; x <= meshBounds.xmax; x++) { + voxelArea.ResolveSolid(z*voxelArea.width + x, m, voxelWalkableClimb); + } + } + } + } + } + } + + [BurstCompile(CompileSynchronously = true)] + struct JobBuildCompactField : IJob { + public LinkedVoxelField input; + public CompactVoxelField output; + + public void Execute () { + output.BuildFromLinkedField(input); + } + } + + + [BurstCompile(CompileSynchronously = true)] + struct JobBuildConnections : IJob { + public CompactVoxelField field; + public int voxelWalkableHeight; + public int voxelWalkableClimb; + + public void Execute () { + int wd = field.width*field.depth; + + // Build voxel connections + for (int z = 0, pz = 0; z < wd; z += field.width, pz++) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell c = field.cells[x+z]; + + for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; i++) { + CompactVoxelSpan s = field.spans[i]; + s.con = 0xFFFFFFFF; + + for (int d = 0; d < 4; d++) { + int nx = x+VoxelUtilityBurst.DX[d]; + int nz = z+VoxelUtilityBurst.DZ[d]*field.width; + + if (nx < 0 || nz < 0 || nz >= wd || nx >= field.width) { + continue; + } + + CompactVoxelCell nc = field.cells[nx+nz]; + + for (int k = nc.index, nk = (int)(nc.index+nc.count); k < nk; k++) { + CompactVoxelSpan ns = field.spans[k]; + + int bottom = System.Math.Max(s.y, ns.y); + + int top = System.Math.Min((int)s.y+(int)s.h, (int)ns.y+(int)ns.h); + + if ((top-bottom) >= voxelWalkableHeight && System.Math.Abs((int)ns.y - (int)s.y) <= voxelWalkableClimb) { + uint connIdx = (uint)k - (uint)nc.index; + + if (connIdx > CompactVoxelField.MaxLayers) { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + throw new System.Exception("Too many layers"); +#else + break; +#endif + } + + s.SetConnection(d, connIdx); + break; + } + } + } + + field.spans[i] = s; + } + } + } + } + } + + [BurstCompile(CompileSynchronously = true)] + struct JobErodeWalkableArea : IJob { + public CompactVoxelField field; + public int radius; + + public void Execute () { + var distances = new NativeArray(field.spans.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + VoxelUtilityBurst.CalculateDistanceField(field, distances); + + for (int i = 0; i < distances.Length; i++) { + // Note multiplied with 2 because the distance field increments distance by 2 for each voxel (and 3 for diagonal) + if (distances[i] < radius*2) { + field.areaTypes[i] = CompactVoxelField.UnwalkableArea; + } + } + } + } + + [BurstCompile(CompileSynchronously = true)] + struct JobBuildDistanceField : IJob { + public CompactVoxelField field; + public NativeList output; + + public void Execute () { + var distances = new NativeArray(field.spans.Length, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + VoxelUtilityBurst.CalculateDistanceField(field, distances); + + output.ResizeUninitialized(field.spans.Length); + VoxelUtilityBurst.BoxBlur(field, distances, output.AsArray()); + } + } + + [BurstCompile(CompileSynchronously = true)] + struct JobFilterLowHeightSpans : IJob { + public LinkedVoxelField field; + public uint voxelWalkableHeight; + + public void Execute () { + int wd = field.width*field.depth; + //Filter all ledges + var spans = field.linkedSpans; + + for (int z = 0, pz = 0; z < wd; z += field.width, pz++) { + for (int x = 0; x < field.width; x++) { + for (int s = z+x; s != -1 && spans[s].bottom != LinkedVoxelField.InvalidSpanValue; s = spans[s].next) { + uint bottom = spans[s].top; + uint top = spans[s].next != -1 ? spans[spans[s].next].bottom : LinkedVoxelField.MaxHeight; + + if (top - bottom < voxelWalkableHeight) { + var span = spans[s]; + span.area = CompactVoxelField.UnwalkableArea; + spans[s] = span; + } + } + } + } + } + } + + [BurstCompile(CompileSynchronously = true)] + struct JobFilterLedges : IJob { + public LinkedVoxelField field; + public uint voxelWalkableHeight; + public int voxelWalkableClimb; + public float cellSize; + public float cellHeight; + + // Code almost completely ripped from Recast + public void Execute () { + // Use an UnsafeSpan to be able to use the ref-return values in order to directly assign fields on spans. + var spans = field.linkedSpans.AsUnsafeSpan(); + int wd = field.width*field.depth; + int width = field.width; + + // Filter all ledges + for (int z = 0, pz = 0; z < wd; z += width, pz++) { + for (int x = 0; x < width; x++) { + if (spans[x+z].bottom == LinkedVoxelField.InvalidSpanValue) continue; + + for (int s = x+z; s != -1; s = spans[s].next) { + // Skip non-walkable spans + if (spans[s].area == CompactVoxelField.UnwalkableArea) { + continue; + } + + // Points on the edge of the voxel field will always have at least 1 out-of-bounds neighbour + if (x == 0 || z == 0 || z == (wd-width) || x == (width-1)) { + spans[s].area = CompactVoxelField.UnwalkableArea; + continue; + } + + int bottom = (int)spans[s].top; + int top = spans[s].next != -1 ? (int)spans[spans[s].next].bottom : (int)LinkedVoxelField.MaxHeight; + + // Find neighbours' minimum height. + int minNeighborHeight = (int)LinkedVoxelField.MaxHeight; + + // Min and max height of accessible neighbours. + int accessibleNeighborMinHeight = (int)spans[s].top; + int accessibleNeighborMaxHeight = accessibleNeighborMinHeight; + + for (int d = 0; d < 4; d++) { + int nx = x + VoxelUtilityBurst.DX[d]; + int nz = z + VoxelUtilityBurst.DZ[d]*width; + + int nsx = nx+nz; + + int nbottom = -voxelWalkableClimb; + int ntop = spans[nsx].bottom != LinkedVoxelField.InvalidSpanValue ? (int)spans[nsx].bottom : (int)LinkedVoxelField.MaxHeight; + + // Skip neighbour if the gap between the spans is too small. + if (math.min(top, ntop) - math.max(bottom, nbottom) > voxelWalkableHeight) { + minNeighborHeight = math.min(minNeighborHeight, nbottom - bottom); + } + + // Loop through the rest of the spans + if (spans[nsx].bottom != LinkedVoxelField.InvalidSpanValue) { + for (int ns = nsx; ns != -1; ns = spans[ns].next) { + ref var nSpan = ref spans[ns]; + nbottom = (int)nSpan.top; + + // Break the loop if it is no longer possible for the spans to overlap. + // This is purely a performance optimization + if (nbottom > top - voxelWalkableHeight) break; + + ntop = nSpan.next != -1 ? (int)spans[nSpan.next].bottom : (int)LinkedVoxelField.MaxHeight; + + // Check the overlap of the ranges (bottom,top) and (nbottom,ntop) + // This is the minimum height when moving from the top surface of span #s to the top surface of span #ns + if (math.min(top, ntop) - math.max(bottom, nbottom) > voxelWalkableHeight) { + minNeighborHeight = math.min(minNeighborHeight, nbottom - bottom); + + // Find min/max accessible neighbour height. + if (math.abs(nbottom - bottom) <= voxelWalkableClimb) { + if (nbottom < accessibleNeighborMinHeight) { accessibleNeighborMinHeight = nbottom; } + if (nbottom > accessibleNeighborMaxHeight) { accessibleNeighborMaxHeight = nbottom; } + } + } + } + } + } + + // The current span is close to a ledge if the drop to any + // neighbour span is less than the walkableClimb. + // Additionally, if the difference between all neighbours is too large, + // we are at steep slope: mark the span as ledge. + if (minNeighborHeight < -voxelWalkableClimb || (accessibleNeighborMaxHeight - accessibleNeighborMinHeight) > voxelWalkableClimb) { + spans[s].area = CompactVoxelField.UnwalkableArea; + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs.meta new file mode 100644 index 0000000..7c1c36e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRasterization.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: af78ae0fb20c2907695f4acc47d811a1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs new file mode 100644 index 0000000..0e40a38 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs @@ -0,0 +1,813 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Mathematics; +using Unity.Jobs; +using Unity.Burst; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Navmesh.Voxelization.Burst { + [BurstCompile(CompileSynchronously = true)] + public struct JobBuildRegions : IJob { + public CompactVoxelField field; + public NativeList distanceField; + public int borderSize; + public int minRegionSize; + public NativeQueue srcQue; + public NativeQueue dstQue; + public RecastGraph.RelevantGraphSurfaceMode relevantGraphSurfaceMode; + public NativeArray relevantGraphSurfaces; + + public float cellSize, cellHeight; + public Matrix4x4 graphTransform; + public Bounds graphSpaceBounds; + + void MarkRectWithRegion (int minx, int maxx, int minz, int maxz, ushort region, NativeArray srcReg) { + int md = maxz * field.width; + + for (int z = minz*field.width; z < md; z += field.width) { + for (int x = minx; x < maxx; x++) { + CompactVoxelCell c = field.cells[z+x]; + + for (int i = c.index, ni = c.index+c.count; i < ni; i++) { + if (field.areaTypes[i] != CompactVoxelField.UnwalkableArea) { + srcReg[i] = region; + } + } + } + } + } + + public static bool FloodRegion (int x, int z, int i, uint level, ushort r, + CompactVoxelField field, + NativeArray distanceField, + NativeArray srcReg, + NativeArray srcDist, + NativeArray stack, + NativeArray flags, + NativeArray closed) { + int area = field.areaTypes[i]; + + // Flood f mark region. + int stackSize = 1; + + stack[0] = new Int3 { + x = x, + y = i, + z = z, + }; + + srcReg[i] = r; + srcDist[i] = 0; + + int lev = (int)(level >= 2 ? level-2 : 0); + + int count = 0; + + // Store these in local variables (for performance, avoids an extra indirection) + var compactCells = field.cells; + var compactSpans = field.spans; + var areaTypes = field.areaTypes; + + while (stackSize > 0) { + stackSize--; + var c = stack[stackSize]; + //Similar to the Pop operation of an array, but Pop is not implemented in List<> + int ci = c.y; + int cx = c.x; + int cz = c.z; + + CompactVoxelSpan cs = compactSpans[ci]; + + //Debug.DrawRay (ConvertPosition(cx,cz,ci),Vector3.up, Color.cyan); + + // Check if any of the neighbours already have a valid region set. + ushort ar = 0; + + // Loop through four neighbours + // then check one neighbour of the neighbour + // to get the diagonal neighbour + for (int dir = 0; dir < 4; dir++) { + // 8 connected + if (cs.GetConnection(dir) != CompactVoxelField.NotConnected) { + int ax = cx + VoxelUtilityBurst.DX[dir]; + int az = cz + VoxelUtilityBurst.DZ[dir]*field.width; + + int ai = (int)compactCells[ax+az].index + cs.GetConnection(dir); + + if (areaTypes[ai] != area) + continue; + + ushort nr = srcReg[ai]; + + if ((nr & VoxelUtilityBurst.BorderReg) == VoxelUtilityBurst.BorderReg) // Do not take borders into account. + continue; + + if (nr != 0 && nr != r) { + ar = nr; + // Found a valid region, skip checking the rest + break; + } + + // Rotate dir 90 degrees + int dir2 = (dir+1) & 0x3; + var neighbour2 = compactSpans[ai].GetConnection(dir2); + // Check the diagonal connection + if (neighbour2 != CompactVoxelField.NotConnected) { + int ax2 = ax + VoxelUtilityBurst.DX[dir2]; + int az2 = az + VoxelUtilityBurst.DZ[dir2]*field.width; + + int ai2 = compactCells[ax2+az2].index + neighbour2; + + if (areaTypes[ai2] != area) + continue; + + ushort nr2 = srcReg[ai2]; + + if ((nr2 & VoxelUtilityBurst.BorderReg) == VoxelUtilityBurst.BorderReg) // Do not take borders into account. + continue; + + if (nr2 != 0 && nr2 != r) { + ar = nr2; + // Found a valid region, skip checking the rest + break; + } + } + } + } + + if (ar != 0) { + srcReg[ci] = 0; + srcDist[ci] = 0xFFFF; + continue; + } + count++; + closed[ci] = true; + + + // Expand neighbours. + for (int dir = 0; dir < 4; ++dir) { + if (cs.GetConnection(dir) == CompactVoxelField.NotConnected) continue; + int ax = cx + VoxelUtilityBurst.DX[dir]; + int az = cz + VoxelUtilityBurst.DZ[dir]*field.width; + int ai = compactCells[ax+az].index + cs.GetConnection(dir); + + if (areaTypes[ai] != area) continue; + if (srcReg[ai] != 0) continue; + + if (distanceField[ai] >= lev && flags[ai] == 0) { + srcReg[ai] = r; + srcDist[ai] = 0; + + stack[stackSize] = new Int3 { + x = ax, + y = ai, + z = az, + }; + stackSize++; + } else { + flags[ai] = r; + srcDist[ai] = 2; + } + } + } + + + return count > 0; + } + + public void Execute () { + srcQue.Clear(); + dstQue.Clear(); + + /*System.Diagnostics.Stopwatch w0 = new System.Diagnostics.Stopwatch(); + System.Diagnostics.Stopwatch w1 = new System.Diagnostics.Stopwatch(); + System.Diagnostics.Stopwatch w2 = new System.Diagnostics.Stopwatch(); + System.Diagnostics.Stopwatch w3 = new System.Diagnostics.Stopwatch(); + System.Diagnostics.Stopwatch w4 = new System.Diagnostics.Stopwatch(); + System.Diagnostics.Stopwatch w5 = new System.Diagnostics.Stopwatch(); + w3.Start();*/ + + int w = field.width; + int d = field.depth; + int wd = w*d; + int spanCount = field.spans.Length; + + int expandIterations = 8; + + var srcReg = new NativeArray(spanCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var srcDist = new NativeArray(spanCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var closed = new NativeArray(spanCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var spanFlags = new NativeArray(spanCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + var stack = new NativeArray(spanCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + // The array pool arrays may contain arbitrary data. We need to zero it out. + for (int i = 0; i < spanCount; i++) { + srcReg[i] = 0; + srcDist[i] = 0xFFFF; + closed[i] = false; + spanFlags[i] = 0; + } + + var spanDistances = distanceField; + var areaTypes = field.areaTypes; + var compactCells = field.cells; + const ushort BorderReg = VoxelUtilityBurst.BorderReg; + + ushort regionId = 2; + MarkRectWithRegion(0, borderSize, 0, d, (ushort)(regionId | BorderReg), srcReg); regionId++; + MarkRectWithRegion(w-borderSize, w, 0, d, (ushort)(regionId | BorderReg), srcReg); regionId++; + MarkRectWithRegion(0, w, 0, borderSize, (ushort)(regionId | BorderReg), srcReg); regionId++; + MarkRectWithRegion(0, w, d-borderSize, d, (ushort)(regionId | BorderReg), srcReg); regionId++; + + // TODO: Can be optimized + int maxDistance = 0; + for (int i = 0; i < distanceField.Length; i++) { + maxDistance = math.max(distanceField[i], maxDistance); + } + + // A distance is 2 to an adjacent span and 1 for a diagonally adjacent one. + NativeArray sortedSpanCounts = new NativeArray((maxDistance)/2 + 1, Allocator.Temp); + for (int i = 0; i < field.spans.Length; i++) { + // Do not take borders or unwalkable spans into account. + if ((srcReg[i] & BorderReg) == BorderReg || areaTypes[i] == CompactVoxelField.UnwalkableArea) + continue; + + sortedSpanCounts[distanceField[i]/2]++; + } + + var distanceIndexOffsets = new NativeArray(sortedSpanCounts.Length, Allocator.Temp); + for (int i = 1; i < distanceIndexOffsets.Length; i++) { + distanceIndexOffsets[i] = distanceIndexOffsets[i-1] + sortedSpanCounts[i-1]; + } + var totalRelevantSpans = distanceIndexOffsets[distanceIndexOffsets.Length - 1] + sortedSpanCounts[sortedSpanCounts.Length - 1]; + + var bucketSortedSpans = new NativeArray(totalRelevantSpans, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + // Bucket sort the spans based on distance + for (int z = 0, pz = 0; z < wd; z += w, pz++) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell c = compactCells[z+x]; + + for (int i = c.index, ni = c.index+c.count; i < ni; i++) { + // Do not take borders or unwalkable spans into account. + if ((srcReg[i] & BorderReg) == BorderReg || areaTypes[i] == CompactVoxelField.UnwalkableArea) + continue; + + int distIndex = distanceField[i] / 2; + bucketSortedSpans[distanceIndexOffsets[distIndex]++] = new Int3(x, i, z); + } + } + } + +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (distanceIndexOffsets[distanceIndexOffsets.Length - 1] != totalRelevantSpans) throw new System.Exception("Unexpected span count"); +#endif + + // Go through spans in reverse order (i.e largest distances first) + for (int distIndex = sortedSpanCounts.Length - 1; distIndex >= 0; distIndex--) { + var level = (uint)distIndex * 2; + var spansAtLevel = sortedSpanCounts[distIndex]; + for (int i = 0; i < spansAtLevel; i++) { + // Go through the spans stored in bucketSortedSpans for this distance index. + // Note that distanceIndexOffsets[distIndex] will point to the element after the end of the group of spans. + // There is no particular reason for this, the code just turned out to be a bit simpler to implemen that way. + var spanInfo = bucketSortedSpans[distanceIndexOffsets[distIndex] - i - 1]; + int spanIndex = spanInfo.y; + + // This span is adjacent to a region, so we should start the BFS search from it + if (spanFlags[spanIndex] != 0 && srcReg[spanIndex] == 0) { + srcReg[spanIndex] = (ushort)spanFlags[spanIndex]; + srcQue.Enqueue(spanInfo); + closed[spanIndex] = true; + } + } + + // Expand a few iterations out from every known node + for (int expansionIteration = 0; expansionIteration < expandIterations && srcQue.Count > 0; expansionIteration++) { + while (srcQue.Count > 0) { + Int3 spanInfo = srcQue.Dequeue(); + var area = areaTypes[spanInfo.y]; + var span = field.spans[spanInfo.y]; + var region = srcReg[spanInfo.y]; + closed[spanInfo.y] = true; + ushort nextDist = (ushort)(srcDist[spanInfo.y] + 2); + + // Go through the neighbours of the span + for (int dir = 0; dir < 4; dir++) { + var neighbour = span.GetConnection(dir); + if (neighbour == CompactVoxelField.NotConnected) continue; + + int nx = spanInfo.x + VoxelUtilityBurst.DX[dir]; + int nz = spanInfo.z + VoxelUtilityBurst.DZ[dir]*field.width; + + int ni = compactCells[nx+nz].index + neighbour; + + if ((srcReg[ni] & BorderReg) == BorderReg) // Do not take borders into account. + continue; + + // Do not combine different area types + if (area == areaTypes[ni]) { + if (nextDist < srcDist[ni]) { + if (spanDistances[ni] < level) { + srcDist[ni] = nextDist; + spanFlags[ni] = region; + } else if (!closed[ni]) { + srcDist[ni] = nextDist; + if (srcReg[ni] == 0) dstQue.Enqueue(new Int3(nx, ni, nz)); + srcReg[ni] = region; + } + } + } + } + } + Memory.Swap(ref srcQue, ref dstQue); + } + + // Find the first span that has not been seen yet and start a new region that expands from there + var distanceFieldArr = distanceField.AsArray(); + for (int i = 0; i < spansAtLevel; i++) { + var info = bucketSortedSpans[distanceIndexOffsets[distIndex] - i - 1]; + if (srcReg[info.y] == 0) { + if (!FloodRegion(info.x, info.z, info.y, level, regionId, field, distanceFieldArr, srcReg, srcDist, stack, spanFlags, closed)) { + // The starting voxel was already adjacent to an existing region so we skip flooding it. + // It will be visited in the next area expansion. + } else { + regionId++; + } + } + } + } + + var maxRegions = regionId; + + // Transform from voxel space to graph space. + // then scale from voxel space (one unit equals one voxel) + // Finally add min + Matrix4x4 voxelMatrix = Matrix4x4.TRS(graphSpaceBounds.min, Quaternion.identity, Vector3.one) * Matrix4x4.Scale(new Vector3(cellSize, cellHeight, cellSize)); + + // Transform from voxel space to world space + // add half a voxel to fix rounding + var voxel2worldMatrix = graphTransform * voxelMatrix * Matrix4x4.Translate(new Vector3(0.5f, 0, 0.5f)); + + // Filter out small regions. + FilterSmallRegions(field, srcReg, minRegionSize, maxRegions, this.relevantGraphSurfaces, this.relevantGraphSurfaceMode, voxel2worldMatrix); + + // Write the result out. + for (int i = 0; i < spanCount; i++) { + var span = field.spans[i]; + span.reg = srcReg[i]; + field.spans[i] = span; + } + + // TODO: + // field.maxRegions = maxRegions; + +// #if ASTAR_DEBUGREPLAY +// DebugReplay.BeginGroup("Regions"); +// for (int z = 0, pz = 0; z < wd; z += field.width, pz++) { +// for (int x = 0; x < field.width; x++) { +// CompactVoxelCell c = field.cells[x+z]; +// for (int i = (int)c.index; i < c.index+c.count; i++) { +// CompactVoxelSpan s = field.spans[i]; +// DebugReplay.DrawCube(CompactSpanToVector(x, pz, i), UnityEngine.Vector3.one*cellSize, AstarMath.IntToColor(s.reg, 1.0f)); +// } +// } +// } + +// DebugReplay.EndGroup(); + +// int maxDist = 0; +// for (int i = 0; i < srcDist.Length; i++) if (srcDist[i] != 0xFFFF) maxDist = Mathf.Max(maxDist, srcDist[i]); + +// DebugReplay.BeginGroup("Distances"); +// for (int z = 0, pz = 0; z < wd; z += field.width, pz++) { +// for (int x = 0; x < field.width; x++) { +// CompactVoxelCell c = field.cells[x+z]; +// for (int i = (int)c.index; i < c.index+c.count; i++) { +// CompactVoxelSpan s = field.spans[i]; +// float f = (float)srcDist[i]/maxDist; +// DebugReplay.DrawCube(CompactSpanToVector(x, z/field.width, i), Vector3.one*cellSize, new Color(f, f, f)); +// } +// } +// } + +// DebugReplay.EndGroup(); +// #endif + } + + /// + /// Find method in the UnionFind data structure. + /// See: https://en.wikipedia.org/wiki/Disjoint-set_data_structure + /// + static int union_find_find (NativeArray arr, int x) { + if (arr[x] < 0) return x; + return arr[x] = union_find_find(arr, arr[x]); + } + + /// + /// Join method in the UnionFind data structure. + /// See: https://en.wikipedia.org/wiki/Disjoint-set_data_structure + /// + static void union_find_union (NativeArray arr, int a, int b) { + a = union_find_find(arr, a); + b = union_find_find(arr, b); + if (a == b) return; + if (arr[a] > arr[b]) { + int tmp = a; + a = b; + b = tmp; + } + arr[a] += arr[b]; + arr[b] = a; + } + + public struct RelevantGraphSurfaceInfo { + public float3 position; + public float range; + } + + /// Filters out or merges small regions. + public static void FilterSmallRegions (CompactVoxelField field, NativeArray reg, int minRegionSize, int maxRegions, NativeArray relevantGraphSurfaces, RecastGraph.RelevantGraphSurfaceMode relevantGraphSurfaceMode, float4x4 voxel2worldMatrix) { + // RelevantGraphSurface c = RelevantGraphSurface.Root; + // Need to use ReferenceEquals because it might be called from another thread + bool anySurfaces = relevantGraphSurfaces.Length != 0 && (relevantGraphSurfaceMode != RecastGraph.RelevantGraphSurfaceMode.DoNotRequire); + + // Nothing to do here + if (!anySurfaces && minRegionSize <= 0) { + return; + } + + var counter = new NativeArray(maxRegions, Allocator.Temp); + var bits = new NativeArray(maxRegions, Allocator.Temp, NativeArrayOptions.ClearMemory); + for (int i = 0; i < counter.Length; i++) counter[i] = -1; + + int nReg = counter.Length; + + int wd = field.width*field.depth; + + const int RelevantSurfaceSet = 1 << 1; + const int BorderBit = 1 << 0; + + // Mark RelevantGraphSurfaces + + const ushort BorderReg = VoxelUtilityBurst.BorderReg; + // If they can also be adjacent to tile borders, this will also include the BorderBit + int RelevantSurfaceCheck = RelevantSurfaceSet | ((relevantGraphSurfaceMode == RecastGraph.RelevantGraphSurfaceMode.OnlyForCompletelyInsideTile) ? BorderBit : 0x0); + // int RelevantSurfaceCheck = 0; + + if (anySurfaces) { + var world2voxelMatrix = math.inverse(voxel2worldMatrix); + for (int j = 0; j < relevantGraphSurfaces.Length; j++) { + var relevantGraphSurface = relevantGraphSurfaces[j]; + var positionInVoxelSpace = math.transform(world2voxelMatrix, relevantGraphSurface.position); + int3 cellIndex = (int3)math.round(positionInVoxelSpace); + + // Check for out of bounds + if (cellIndex.x >= 0 && cellIndex.z >= 0 && cellIndex.x < field.width && cellIndex.z < field.depth) { + var yScaleFactor = math.length(voxel2worldMatrix.c1.xyz); + int rad = (int)(relevantGraphSurface.range / yScaleFactor); + + CompactVoxelCell cell = field.cells[cellIndex.x+cellIndex.z*field.width]; + for (int i = cell.index; i < cell.index+cell.count; i++) { + CompactVoxelSpan s = field.spans[i]; + if (System.Math.Abs(s.y - cellIndex.y) <= rad && reg[i] != 0) { + bits[union_find_find(counter, reg[i] & ~BorderReg)] |= RelevantSurfaceSet; + } + } + } + } + } + + for (int z = 0; z < wd; z += field.width) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell cell = field.cells[x+z]; + + for (int i = cell.index; i < cell.index+cell.count; i++) { + CompactVoxelSpan s = field.spans[i]; + + int r = reg[i]; + + // Check if this is an unwalkable span + if ((r & ~BorderReg) == 0) continue; + + if (r >= nReg) { //Probably border + bits[union_find_find(counter, r & ~BorderReg)] |= BorderBit; + continue; + } + + int root = union_find_find(counter, r); + // Count this span + counter[root]--; + + // Iterate through all neighbours of the span. + for (int dir = 0; dir < 4; dir++) { + if (s.GetConnection(dir) == CompactVoxelField.NotConnected) { continue; } + + int nx = x + VoxelUtilityBurst.DX[dir]; + int nz = z + VoxelUtilityBurst.DZ[dir] * field.width; + + int ni = field.cells[nx+nz].index + s.GetConnection(dir); + + int r2 = reg[ni]; + + // Check if the other span belongs to a different region and is walkable + if (r != r2 && (r2 & ~BorderReg) != 0) { + if ((r2 & BorderReg) != 0) { + // If it's a border region we just mark the current region as being adjacent to a border + bits[root] |= BorderBit; + } else { + // Join the adjacent region with this region. + union_find_union(counter, root, r2); + } + //counter[r] = minRegionSize; + } + } + //counter[r]++; + } + } + } + + // Propagate bits to the region group representative using the union find structure + for (int i = 0; i < counter.Length; i++) bits[union_find_find(counter, i)] |= bits[i]; + + for (int i = 0; i < counter.Length; i++) { + int ctr = union_find_find(counter, i); + + // Check if the region is adjacent to border. + // Mark it as being just large enough to always be included in the graph. + if ((bits[ctr] & BorderBit) != 0) counter[ctr] = -minRegionSize-2; + + // Not in any relevant surface + // or it is adjacent to a border (see RelevantSurfaceCheck) + if (anySurfaces && (bits[ctr] & RelevantSurfaceCheck) == 0) counter[ctr] = -1; + } + + for (int i = 0; i < reg.Length; i++) { + int r = reg[i]; + // Ignore border regions + if (r >= nReg) { + continue; + } + + // If the region group is too small then make the span unwalkable + if (counter[union_find_find(counter, r)] >= -minRegionSize-1) { + reg[i] = 0; + } + } + } + } + + static class VoxelUtilityBurst { + /// All bits in the region which will be interpreted as a tag. + public const int TagRegMask = TagReg - 1; + + /// + /// If a cell region has this bit set then + /// The remaining region bits (see will be used for the node's tag. + /// + public const int TagReg = 1 << 14; + + /// + /// If heightfield region ID has the following bit set, the region is on border area + /// and excluded from many calculations. + /// + public const ushort BorderReg = 1 << 15; + + /// + /// If contour region ID has the following bit set, the vertex will be later + /// removed in order to match the segments and vertices at tile boundaries. + /// + public const int RC_BORDER_VERTEX = 1 << 16; + + public const int RC_AREA_BORDER = 1 << 17; + + public const int VERTEX_BUCKET_COUNT = 1<<12; + + /// Tessellate wall edges + public const int RC_CONTOUR_TESS_WALL_EDGES = 1 << 0; + + /// Tessellate edges between areas + public const int RC_CONTOUR_TESS_AREA_EDGES = 1 << 1; + + /// Tessellate edges at the border of the tile + public const int RC_CONTOUR_TESS_TILE_EDGES = 1 << 2; + + /// Mask used with contours to extract region id. + public const int ContourRegMask = 0xffff; + + public static readonly int[] DX = new int[] { -1, 0, 1, 0 }; + public static readonly int[] DZ = new int[] { 0, 1, 0, -1 }; + + public static void CalculateDistanceField (CompactVoxelField field, NativeArray output) { + int wd = field.width*field.depth; + + // Mark boundary cells + for (int z = 0; z < wd; z += field.width) { + for (int x = 0; x < field.width; x++) { + CompactVoxelCell c = field.cells[x+z]; + + for (int i = c.index, ci = c.index+c.count; i < ci; i++) { + CompactVoxelSpan s = field.spans[i]; + + int numConnections = 0; + for (int d = 0; d < 4; d++) { + if (s.GetConnection(d) != CompactVoxelField.NotConnected) { + //This function (CalculateDistanceField) is used for both ErodeWalkableArea and by itself. + //The C++ recast source uses different code for those two cases, but I have found it works with one function + //the field.areaTypes[ni] will actually only be one of two cases when used from ErodeWalkableArea + //so it will have the same effect as + // if (area != UnwalkableArea) { + //This line is the one where the differ most + + numConnections++; + } else { + break; + } + } + + // TODO: Check initialization + output[i] = numConnections == 4 ? ushort.MaxValue : (ushort)0; + } + } + } + + // Grassfire transform + // Pass 1 + + for (int z = 0; z < wd; z += field.width) { + for (int x = 0; x < field.width; x++) { + int cellIndex = x + z; + CompactVoxelCell c = field.cells[cellIndex]; + + for (int i = c.index, ci = c.index+c.count; i < ci; i++) { + CompactVoxelSpan s = field.spans[i]; + var dist = (int)output[i]; + + if (s.GetConnection(0) != CompactVoxelField.NotConnected) { + // (-1,0) + int neighbourCell = field.GetNeighbourIndex(cellIndex, 0); + + int ni = field.cells[neighbourCell].index+s.GetConnection(0); + + dist = math.min(dist, (int)output[ni]+2); + + CompactVoxelSpan ns = field.spans[ni]; + + if (ns.GetConnection(3) != CompactVoxelField.NotConnected) { + // (-1,0) + (0,-1) = (-1,-1) + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, 3); + + int nni = (int)(field.cells[neighbourCell2].index+ns.GetConnection(3)); + + dist = math.min(dist, (int)output[nni]+3); + } + } + + if (s.GetConnection(3) != CompactVoxelField.NotConnected) { + // (0,-1) + int neighbourCell = field.GetNeighbourIndex(cellIndex, 3); + + int ni = (int)(field.cells[neighbourCell].index+s.GetConnection(3)); + + dist = math.min(dist, (int)output[ni]+2); + + CompactVoxelSpan ns = field.spans[ni]; + + if (ns.GetConnection(2) != CompactVoxelField.NotConnected) { + // (0,-1) + (1,0) = (1,-1) + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, 2); + + int nni = (int)(field.cells[neighbourCell2].index+ns.GetConnection(2)); + + dist = math.min(dist, (int)output[nni]+3); + } + } + + output[i] = (ushort)dist; + } + } + } + + // Pass 2 + + for (int z = wd-field.width; z >= 0; z -= field.width) { + for (int x = field.width-1; x >= 0; x--) { + int cellIndex = x + z; + CompactVoxelCell c = field.cells[cellIndex]; + + for (int i = (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) { + CompactVoxelSpan s = field.spans[i]; + var dist = (int)output[i]; + + if (s.GetConnection(2) != CompactVoxelField.NotConnected) { + // (-1,0) + int neighbourCell = field.GetNeighbourIndex(cellIndex, 2); + + int ni = (int)(field.cells[neighbourCell].index+s.GetConnection(2)); + + dist = math.min(dist, (int)output[ni]+2); + + CompactVoxelSpan ns = field.spans[ni]; + + if (ns.GetConnection(1) != CompactVoxelField.NotConnected) { + // (-1,0) + (0,-1) = (-1,-1) + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, 1); + + int nni = (int)(field.cells[neighbourCell2].index+ns.GetConnection(1)); + + dist = math.min(dist, (int)output[nni]+3); + } + } + + if (s.GetConnection(1) != CompactVoxelField.NotConnected) { + // (0,-1) + int neighbourCell = field.GetNeighbourIndex(cellIndex, 1); + + int ni = (int)(field.cells[neighbourCell].index+s.GetConnection(1)); + + dist = math.min(dist, (int)output[ni]+2); + + CompactVoxelSpan ns = field.spans[ni]; + + if (ns.GetConnection(0) != CompactVoxelField.NotConnected) { + // (0,-1) + (1,0) = (1,-1) + int neighbourCell2 = field.GetNeighbourIndex(neighbourCell, 0); + + int nni = (int)(field.cells[neighbourCell2].index+ns.GetConnection(0)); + + dist = math.min(dist, (int)output[nni]+3); + } + } + + output[i] = (ushort)dist; + } + } + } + +// #if ASTAR_DEBUGREPLAY && FALSE +// DebugReplay.BeginGroup("Distance Field"); +// for (int z = wd-field.width; z >= 0; z -= field.width) { +// for (int x = field.width-1; x >= 0; x--) { +// CompactVoxelCell c = field.cells[x+z]; + +// for (int i = (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) { +// DebugReplay.DrawCube(CompactSpanToVector(x, z/field.width, i), Vector3.one*cellSize, new Color((float)output[i]/maxDist, (float)output[i]/maxDist, (float)output[i]/maxDist)); +// } +// } +// } +// DebugReplay.EndGroup(); +// #endif + } + + public static void BoxBlur (CompactVoxelField field, NativeArray src, NativeArray dst) { + ushort thr = 20; + + int wd = field.width*field.depth; + + for (int z = wd-field.width; z >= 0; z -= field.width) { + for (int x = field.width-1; x >= 0; x--) { + int cellIndex = x + z; + CompactVoxelCell c = field.cells[cellIndex]; + + for (int i = (int)c.index, ci = (int)(c.index+c.count); i < ci; i++) { + CompactVoxelSpan s = field.spans[i]; + + ushort cd = src[i]; + + if (cd < thr) { + dst[i] = cd; + continue; + } + + int total = (int)cd; + + for (int d = 0; d < 4; d++) { + if (s.GetConnection(d) != CompactVoxelField.NotConnected) { + var neighbourIndex = field.GetNeighbourIndex(cellIndex, d); + int ni = (int)(field.cells[neighbourIndex].index+s.GetConnection(d)); + + total += (int)src[ni]; + + CompactVoxelSpan ns = field.spans[ni]; + + int d2 = (d+1) & 0x3; + + if (ns.GetConnection(d2) != CompactVoxelField.NotConnected) { + var neighbourIndex2 = field.GetNeighbourIndex(neighbourIndex, d2); + + int nni = (int)(field.cells[neighbourIndex2].index+ns.GetConnection(d2)); + total += (int)src[nni]; + } else { + total += cd; + } + } else { + total += cd*2; + } + } + dst[i] = (ushort)((total+5)/9F); + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs.meta new file mode 100644 index 0000000..761ad5c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh/Voxels/VoxelRegion.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b5b3bda46dccdc886959894545826304 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: -- cgit v1.1-26-g67d0