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 --- .../com.arongranberg.astar/Graphs/Grid.meta | 8 + .../Graphs/Grid/GraphCollision.cs | 387 ++ .../Graphs/Grid/GraphCollision.cs.meta | 11 + .../Graphs/Grid/GridAdjacencyMapper.cs | 37 + .../Graphs/Grid/GridAdjacencyMapper.cs.meta | 11 + .../Graphs/Grid/GridGraphScanData.cs | 700 ++++ .../Graphs/Grid/GridGraphScanData.cs.meta | 11 + .../Graphs/Grid/GridIterationUtilities.cs | 261 ++ .../Graphs/Grid/GridIterationUtilities.cs.meta | 11 + .../com.arongranberg.astar/Graphs/Grid/Jobs.meta | 8 + .../Graphs/Grid/Jobs/JobAllocateNodes.cs | 57 + .../Graphs/Grid/Jobs/JobAllocateNodes.cs.meta | 11 + .../Grid/Jobs/JobCalculateGridConnections.cs | 219 ++ .../Grid/Jobs/JobCalculateGridConnections.cs.meta | 11 + .../Graphs/Grid/Jobs/JobCheckCollisions.cs | 33 + .../Graphs/Grid/Jobs/JobCheckCollisions.cs.meta | 11 + .../Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs | 28 + .../Grid/Jobs/JobColliderHitsToBooleans.cs.meta | 11 + .../Graphs/Grid/Jobs/JobCopyBuffers.cs | 50 + .../Graphs/Grid/Jobs/JobCopyBuffers.cs.meta | 11 + .../Graphs/Grid/Jobs/JobErosion.cs | 190 + .../Graphs/Grid/Jobs/JobErosion.cs.meta | 11 + .../Grid/Jobs/JobFilterDiagonalConnections.cs | 128 + .../Grid/Jobs/JobFilterDiagonalConnections.cs.meta | 11 + .../Grid/Jobs/JobMergeRaycastCollisionHits.cs | 31 + .../Grid/Jobs/JobMergeRaycastCollisionHits.cs.meta | 11 + .../Graphs/Grid/Jobs/JobNodeGridLayout.cs | 36 + .../Graphs/Grid/Jobs/JobNodeGridLayout.cs.meta | 11 + .../Graphs/Grid/Jobs/JobNodeWalkability.cs | 73 + .../Graphs/Grid/Jobs/JobNodeWalkability.cs.meta | 11 + .../Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs | 45 + .../Grid/Jobs/JobPrepareCapsuleCommands.cs.meta | 11 + .../Graphs/Grid/Jobs/JobPrepareGridRaycast.cs | 61 + .../Graphs/Grid/Jobs/JobPrepareGridRaycast.cs.meta | 11 + .../Graphs/Grid/Jobs/JobPrepareRaycasts.cs | 51 + .../Graphs/Grid/Jobs/JobPrepareRaycasts.cs.meta | 11 + .../Graphs/Grid/Jobs/JobPrepareSphereCommands.cs | 42 + .../Grid/Jobs/JobPrepareSphereCommands.cs.meta | 11 + .../Graphs/Grid/Jobs/JobRaycastAll.cs | 121 + .../Graphs/Grid/Jobs/JobRaycastAll.cs.meta | 11 + .../Graphs/Grid/Jobs/JobReadNodeData.cs | 93 + .../Graphs/Grid/Jobs/JobReadNodeData.cs.meta | 11 + .../Graphs/Grid/Jobs/JobWriteNodeData.cs | 91 + .../Graphs/Grid/Jobs/JobWriteNodeData.cs.meta | 11 + .../com.arongranberg.astar/Graphs/Grid/Rules.meta | 8 + .../Graphs/Grid/Rules/GridGraphRules.cs | 293 ++ .../Graphs/Grid/Rules/GridGraphRules.cs.meta | 11 + .../Graphs/Grid/Rules/RuleAnglePenalty.cs | 81 + .../Graphs/Grid/Rules/RuleAnglePenalty.cs.meta | 11 + .../Graphs/Grid/Rules/RuleElevationPenalty.cs | 76 + .../Graphs/Grid/Rules/RuleElevationPenalty.cs.meta | 11 + .../Graphs/Grid/Rules/RulePerLayerModifications.cs | 79 + .../Grid/Rules/RulePerLayerModifications.cs.meta | 11 + .../Graphs/Grid/Rules/RuleTexture.cs | 181 + .../Graphs/Grid/Rules/RuleTexture.cs.meta | 11 + .../com.arongranberg.astar/Graphs/GridGraph.cs | 3681 ++++++++++++++++++++ .../Graphs/GridGraph.cs.meta | 7 + .../Graphs/LayerGridGraph.cs | 246 ++ .../Graphs/LayerGridGraph.cs.meta | 7 + .../com.arongranberg.astar/Graphs/LinkGraph.cs | 167 + .../Graphs/LinkGraph.cs.meta | 11 + .../com.arongranberg.astar/Graphs/NavGraph.cs | 469 +++ .../com.arongranberg.astar/Graphs/NavGraph.cs.meta | 7 + .../com.arongranberg.astar/Graphs/NavMeshGraph.cs | 372 ++ .../Graphs/NavMeshGraph.cs.meta | 7 + .../com.arongranberg.astar/Graphs/Navmesh.meta | 8 + .../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 + .../com.arongranberg.astar/Graphs/NavmeshBase.cs | 1736 +++++++++ .../Graphs/NavmeshBase.cs.meta | 12 + .../com.arongranberg.astar/Graphs/Nodes.meta | 2 + .../Graphs/Nodes/GridNode.cs | 447 +++ .../Graphs/Nodes/GridNode.cs.meta | 7 + .../Graphs/Nodes/GridNodeBase.cs | 534 +++ .../Graphs/Nodes/GridNodeBase.cs.meta | 12 + .../Graphs/Nodes/LevelGridNode.cs | 403 +++ .../Graphs/Nodes/LevelGridNode.cs.meta | 11 + .../Graphs/Nodes/PointNode.cs | 220 ++ .../Graphs/Nodes/PointNode.cs.meta | 8 + .../Graphs/Nodes/TriangleMeshNode.cs | 673 ++++ .../Graphs/Nodes/TriangleMeshNode.cs.meta | 8 + .../com.arongranberg.astar/Graphs/Point.meta | 8 + .../Graphs/Point/PointKDTree.cs | 309 ++ .../Graphs/Point/PointKDTree.cs.meta | 12 + .../com.arongranberg.astar/Graphs/PointGraph.cs | 772 ++++ .../Graphs/PointGraph.cs.meta | 7 + .../com.arongranberg.astar/Graphs/RecastGraph.cs | 1392 ++++++++ .../Graphs/RecastGraph.cs.meta | 7 + .../com.arongranberg.astar/Graphs/Utilities.meta | 2 + .../Graphs/Utilities/EuclideanEmbedding.cs | 442 +++ .../Graphs/Utilities/EuclideanEmbedding.cs.meta | 8 + .../Graphs/Utilities/GraphTransform.cs | 575 +++ .../Graphs/Utilities/GraphTransform.cs.meta | 12 + .../Graphs/Utilities/GridLookup.cs | 220 ++ .../Graphs/Utilities/GridLookup.cs.meta | 12 + .../Graphs/Utilities/NavMeshRenderer.cs | 4 + .../Graphs/Utilities/NavMeshRenderer.cs.meta | 7 + .../Graphs/Utilities/RecastMeshObj.cs | 311 ++ .../Graphs/Utilities/RecastMeshObj.cs.meta | 7 + .../Graphs/Utilities/UtilityJobs.cs | 341 ++ .../Graphs/Utilities/UtilityJobs.cs.meta | 11 + 155 files changed, 26232 insertions(+) create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/GridGraph.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/GridGraph.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LayerGridGraph.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LayerGridGraph.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LinkGraph.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LinkGraph.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavGraph.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavGraph.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavMeshGraph.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavMeshGraph.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh.meta 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 create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavmeshBase.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavmeshBase.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point/PointKDTree.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point/PointKDTree.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/PointGraph.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/PointGraph.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/RecastGraph.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/RecastGraph.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs.meta create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs create mode 100644 Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs.meta (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs') diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid.meta new file mode 100644 index 0000000..0415f7c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 339245ab27a1142a3b80c511ca17ad70 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs new file mode 100644 index 0000000..a21f179 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs @@ -0,0 +1,387 @@ +using UnityEngine; +using System.Collections.Generic; +using Unity.Collections; + +namespace Pathfinding.Graphs.Grid { + using Pathfinding.Util; + using Pathfinding.Graphs.Grid.Jobs; + using Pathfinding.Jobs; + + /// + /// Handles collision checking for graphs. + /// Mostly used by grid based graphs + /// + [System.Serializable] + public class GraphCollision { + /// + /// Collision shape to use. + /// See: + /// + public ColliderType type = ColliderType.Capsule; + + /// + /// Diameter of capsule or sphere when checking for collision. + /// When checking for collisions the system will check if any colliders + /// overlap a specific shape at the node's position. The shape is determined + /// by the field. + /// + /// A diameter of 1 means that the shape has a diameter equal to the node's width, + /// or in other words it is equal to . + /// + /// If is set to Ray, this does not affect anything. + /// + /// [Open online documentation to see images] + /// + public float diameter = 1F; + + /// + /// Height of capsule or length of ray when checking for collision. + /// If is set to Sphere, this does not affect anything. + /// + /// [Open online documentation to see images] + /// + /// Warning: In contrast to Unity's capsule collider and character controller this height does not include the end spheres of the capsule, but only the cylinder part. + /// This is mostly for historical reasons. + /// + public float height = 2F; + + /// + /// Height above the ground that collision checks should be done. + /// For example, if the ground was found at y=0, collisionOffset = 2 + /// type = Capsule and height = 3 then the physics system + /// will be queried to see if there are any colliders in a capsule + /// for which the bottom sphere that is made up of is centered at y=2 + /// and the top sphere has its center at y=2+3=5. + /// + /// If type = Sphere then the sphere's center would be at y=2 in this case. + /// + public float collisionOffset; + + /// + /// Direction of the ray when checking for collision. + /// If is not Ray, this does not affect anything + /// + /// Deprecated: Only the Both mode is supported now. + /// + [System.Obsolete("Only the Both mode is supported now")] + public RayDirection rayDirection = RayDirection.Both; + + /// Layers to be treated as obstacles. + public LayerMask mask; + + /// Layers to be included in the height check. + public LayerMask heightMask = -1; + + /// + /// The height to check from when checking height ('ray length' in the inspector). + /// + /// As the image below visualizes, different ray lengths can make the ray hit different things. + /// The distance is measured up from the graph plane. + /// + /// [Open online documentation to see images] + /// + public float fromHeight = 100; + + /// + /// Toggles thick raycast. + /// See: https://docs.unity3d.com/ScriptReference/Physics.SphereCast.html + /// + public bool thickRaycast; + + /// + /// Diameter of the thick raycast in nodes. + /// 1 equals + /// + public float thickRaycastDiameter = 1; + + /// Make nodes unwalkable when no ground was found with the height raycast. If height raycast is turned off, this doesn't affect anything. + public bool unwalkableWhenNoGround = true; + + /// + /// Use Unity 2D Physics API. + /// + /// If enabled, the 2D Physics API will be used, and if disabled, the 3D Physics API will be used. + /// + /// This changes the collider types (see from 3D versions to their corresponding 2D versions. For example the sphere shape becomes a circle. + /// + /// The setting will be ignored when 2D physics is used. + /// + /// See: http://docs.unity3d.com/ScriptReference/Physics2D.html + /// + public bool use2D; + + /// Toggle collision check + public bool collisionCheck = true; + + /// + /// Toggle height check. If false, the grid will be flat. + /// + /// This setting will be ignored when 2D physics is used. + /// + public bool heightCheck = true; + + /// + /// Direction to use as UP. + /// See: Initialize + /// + public Vector3 up; + + /// + /// * . + /// See: Initialize + /// + private Vector3 upheight; + + /// Used for 2D collision queries + private ContactFilter2D contactFilter; + + /// + /// Just so that the Physics2D.OverlapPoint method has some buffer to store things in. + /// We never actually read from this array, so we don't even care if this is thread safe. + /// + private static Collider2D[] dummyArray = new Collider2D[1]; + + /// + /// * scale * 0.5. + /// Where scale usually is + /// See: Initialize + /// + private float finalRadius; + + /// + /// * scale * 0.5. + /// Where scale usually is See: Initialize + /// + private float finalRaycastRadius; + + /// Offset to apply after each raycast to make sure we don't hit the same point again in CheckHeightAll + public const float RaycastErrorMargin = 0.005F; + + /// + /// Sets up several variables using the specified matrix and scale. + /// See: GraphCollision.up + /// See: GraphCollision.upheight + /// See: GraphCollision.finalRadius + /// See: GraphCollision.finalRaycastRadius + /// + public void Initialize (GraphTransform transform, float scale) { + up = (transform.Transform(Vector3.up) - transform.Transform(Vector3.zero)).normalized; + upheight = up*height; + finalRadius = diameter*scale*0.5F; + finalRaycastRadius = thickRaycastDiameter*scale*0.5F; + contactFilter = new ContactFilter2D { layerMask = mask, useDepth = false, useLayerMask = true, useNormalAngle = false, useTriggers = false }; + } + + /// + /// Returns true if the position is not obstructed. + /// If is false, this will always return true. + /// + public bool Check (Vector3 position) { + if (!collisionCheck) { + return true; + } + + if (use2D) { + switch (type) { + case ColliderType.Capsule: + case ColliderType.Sphere: + return Physics2D.OverlapCircle(position, finalRadius, contactFilter, dummyArray) == 0; + default: + return Physics2D.OverlapPoint(position, contactFilter, dummyArray) == 0; + } + } + + position += up*collisionOffset; + switch (type) { + case ColliderType.Capsule: + return !Physics.CheckCapsule(position, position+upheight, finalRadius, mask, QueryTriggerInteraction.Ignore); + case ColliderType.Sphere: + return !Physics.CheckSphere(position, finalRadius, mask, QueryTriggerInteraction.Ignore); + default: + return !Physics.Raycast(position, up, height, mask, QueryTriggerInteraction.Ignore) && !Physics.Raycast(position+upheight, -up, height, mask, QueryTriggerInteraction.Ignore); + } + } + + /// + /// Returns the position with the correct height. + /// If is false, this will return position. + /// + public Vector3 CheckHeight (Vector3 position) { + RaycastHit hit; + bool walkable; + + return CheckHeight(position, out hit, out walkable); + } + + /// + /// Returns the position with the correct height. + /// If is false, this will return position. + /// walkable will be set to false if nothing was hit. + /// The ray will check a tiny bit further than to the grids base to avoid floating point errors when the ground is exactly at the base of the grid + /// + public Vector3 CheckHeight (Vector3 position, out RaycastHit hit, out bool walkable) { + walkable = true; + + if (!heightCheck || use2D) { + hit = new RaycastHit(); + return position; + } + + if (thickRaycast) { + var ray = new Ray(position+up*fromHeight, -up); + if (Physics.SphereCast(ray, finalRaycastRadius, out hit, fromHeight+0.005F, heightMask, QueryTriggerInteraction.Ignore)) { + return VectorMath.ClosestPointOnLine(ray.origin, ray.origin+ray.direction, hit.point); + } + + walkable &= !unwalkableWhenNoGround; + } else { + // Cast a ray from above downwards to try to find the ground + if (Physics.Raycast(position+up*fromHeight, -up, out hit, fromHeight+0.005F, heightMask, QueryTriggerInteraction.Ignore)) { + return hit.point; + } + + walkable &= !unwalkableWhenNoGround; + } + return position; + } + + /// Internal buffer used by + RaycastHit[] hitBuffer = new RaycastHit[8]; + + /// + /// Returns all hits when checking height for position. + /// Warning: Does not work well with thick raycast, will only return an object a single time + /// + /// Warning: The returned array is ephermal. It will be invalidated when this method is called again. + /// If you need persistent results you should copy it. + /// + /// The returned array may be larger than the actual number of hits, the numHits out parameter indicates how many hits there actually were. + /// + public RaycastHit[] CheckHeightAll (Vector3 position, out int numHits) { + if (!heightCheck || use2D) { + hitBuffer[0] = new RaycastHit { + point = position, + distance = 0, + }; + numHits = 1; + return hitBuffer; + } + + // Cast a ray from above downwards to try to find the ground +#if UNITY_2017_1_OR_NEWER + numHits = Physics.RaycastNonAlloc(position+up*fromHeight, -up, hitBuffer, fromHeight+0.005F, heightMask, QueryTriggerInteraction.Ignore); + if (numHits == hitBuffer.Length) { + // Try again with a larger buffer + hitBuffer = new RaycastHit[hitBuffer.Length*2]; + return CheckHeightAll(position, out numHits); + } + return hitBuffer; +#else + var result = Physics.RaycastAll(position+up*fromHeight, -up, fromHeight+0.005F, heightMask, QueryTriggerInteraction.Ignore); + numHits = result.Length; + return result; +#endif + } + + /// + /// Returns if the position is obstructed for all nodes using the Ray collision checking method. + /// collisionCheckResult[i] = true if there were no obstructions, false otherwise + /// + public void JobCollisionRay (NativeArray nodePositions, NativeArray collisionCheckResult, Vector3 up, Allocator allocationMethod, JobDependencyTracker dependencyTracker) { + var collisionRaycastCommands1 = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + var collisionRaycastCommands2 = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + var collisionHits1 = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + var collisionHits2 = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + + // Fire rays from above down to the nodes' positions + new JobPrepareRaycasts { + origins = nodePositions, + originOffset = up * (height + collisionOffset), + direction = -up, + distance = height, + mask = mask, + physicsScene = Physics.defaultPhysicsScene, + raycastCommands = collisionRaycastCommands1, + }.Schedule(dependencyTracker); + + // Fire rays from the node up towards the sky + new JobPrepareRaycasts { + origins = nodePositions, + originOffset = up * collisionOffset, + direction = up, + distance = height, + mask = mask, + physicsScene = Physics.defaultPhysicsScene, + raycastCommands = collisionRaycastCommands2, + }.Schedule(dependencyTracker); + + dependencyTracker.ScheduleBatch(collisionRaycastCommands1, collisionHits1, 2048); + dependencyTracker.ScheduleBatch(collisionRaycastCommands2, collisionHits2, 2048); + + new JobMergeRaycastCollisionHits { + hit1 = collisionHits1, + hit2 = collisionHits2, + result = collisionCheckResult, + }.Schedule(dependencyTracker); + } + +#if UNITY_2022_2_OR_NEWER + public void JobCollisionCapsule (NativeArray nodePositions, NativeArray collisionCheckResult, Vector3 up, Allocator allocationMethod, JobDependencyTracker dependencyTracker) { + var commands = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + var collisionHits = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + new JobPrepareCapsuleCommands { + origins = nodePositions, + originOffset = up * collisionOffset, + direction = up * height, + radius = finalRadius, + mask = mask, + commands = commands, + physicsScene = Physics.defaultPhysicsScene, + }.Schedule(dependencyTracker); + dependencyTracker.ScheduleBatch(commands, collisionHits, 2048); + new JobColliderHitsToBooleans { + hits = collisionHits, + result = collisionCheckResult, + }.Schedule(dependencyTracker); + } + + public void JobCollisionSphere (NativeArray nodePositions, NativeArray collisionCheckResult, Vector3 up, Allocator allocationMethod, JobDependencyTracker dependencyTracker) { + var commands = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + var collisionHits = dependencyTracker.NewNativeArray(nodePositions.Length, allocationMethod); + new JobPrepareSphereCommands { + origins = nodePositions, + originOffset = up * collisionOffset, + radius = finalRadius, + mask = mask, + commands = commands, + physicsScene = Physics.defaultPhysicsScene, + }.Schedule(dependencyTracker); + dependencyTracker.ScheduleBatch(commands, collisionHits, 2048); + new JobColliderHitsToBooleans { + hits = collisionHits, + result = collisionCheckResult, + }.Schedule(dependencyTracker); + } +#endif + } + + /// + /// Determines collision check shape. + /// See: + /// + public enum ColliderType { + /// Uses a Sphere, Physics.CheckSphere. In 2D this is a circle instead. + Sphere, + /// Uses a Capsule, Physics.CheckCapsule. This will behave identically to the Sphere mode in 2D. + Capsule, + /// Uses a Ray, Physics.Linecast. In 2D this is a single point instead. + Ray + } + + /// Determines collision check ray direction + public enum RayDirection { + Up, /// < Casts the ray from the bottom upwards + Down, /// < Casts the ray from the top downwards + Both /// < Casts two rays in both directions + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs.meta new file mode 100644 index 0000000..373fbfc --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GraphCollision.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b75c44e59ef4c7545af071adedd2ee03 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs new file mode 100644 index 0000000..bba0a60 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs @@ -0,0 +1,37 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Pathfinding.Graphs.Grid { + public interface GridAdjacencyMapper { + int LayerCount(IntBounds bounds); + int GetNeighbourIndex(int nodeIndexXZ, int nodeIndex, int direction, NativeArray nodeConnections, NativeArray neighbourOffsets, int layerStride); + bool HasConnection(int nodeIndex, int direction, NativeArray nodeConnections); + } + + public struct FlatGridAdjacencyMapper : GridAdjacencyMapper { + public int LayerCount (IntBounds bounds) { + UnityEngine.Assertions.Assert.IsTrue(bounds.size.y == 1); + return 1; + } + public int GetNeighbourIndex (int nodeIndexXZ, int nodeIndex, int direction, NativeArray nodeConnections, NativeArray neighbourOffsets, int layerStride) { + return nodeIndex + neighbourOffsets[direction]; + } + public bool HasConnection (int nodeIndex, int direction, NativeArray nodeConnections) { + return ((nodeConnections[nodeIndex] >> direction) & 0x1) != 0; + } + } + + public struct LayeredGridAdjacencyMapper : GridAdjacencyMapper { + public int LayerCount(IntBounds bounds) => bounds.size.y; + public int GetNeighbourIndex (int nodeIndexXZ, int nodeIndex, int direction, NativeArray nodeConnections, NativeArray neighbourOffsets, int layerStride) { + return nodeIndexXZ + neighbourOffsets[direction] + (int)((nodeConnections[nodeIndex] >> LevelGridNode.ConnectionStride*direction) & LevelGridNode.ConnectionMask) * layerStride; + } + public bool HasConnection (int nodeIndex, int direction, NativeArray nodeConnections) { + return ((nodeConnections[nodeIndex] >> LevelGridNode.ConnectionStride*direction) & LevelGridNode.ConnectionMask) != LevelGridNode.NoConnection; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs.meta new file mode 100644 index 0000000..053f5a7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridAdjacencyMapper.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6c5c520c833b3b444b17fcb264c08a7a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs new file mode 100644 index 0000000..e1a4fbb --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs @@ -0,0 +1,700 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using Pathfinding.Util; +using UnityEngine.Profiling; +using System.Collections.Generic; +using Pathfinding.Jobs; +using Pathfinding.Graphs.Grid.Jobs; +using Unity.Jobs.LowLevel.Unsafe; + +namespace Pathfinding.Graphs.Grid { + public struct GridGraphNodeData { + public Allocator allocationMethod; + public int numNodes; + /// + /// Bounds for the part of the graph that this data represents. + /// For example if the first layer of a layered grid graph is being updated between x=10 and x=20, z=5 and z=15 + /// then this will be IntBounds(xmin=10, ymin=0, zmin=5, xmax=20, ymax=0, zmax=15) + /// + public IntBounds bounds; + /// + /// Number of layers that the data contains. + /// For a non-layered grid graph this will always be 1. + /// + public int layers => bounds.size.y; + + /// + /// Positions of all nodes. + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid + /// - BeforeConnections: Valid + /// - AfterConnections: Valid + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// + public NativeArray positions; + + /// + /// Bitpacked connections of all nodes. + /// + /// Connections are stored in different formats depending on . + /// You can use and to access connections for the different data layouts. + /// + /// Data is valid in these passes: + /// - BeforeCollision: Invalid + /// - BeforeConnections: Invalid + /// - AfterConnections: Valid + /// - AfterErosion: Valid (but will be overwritten) + /// - PostProcess: Valid + /// + public NativeArray connections; + + /// + /// Bitpacked connections of all nodes. + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid + /// - BeforeConnections: Valid + /// - AfterConnections: Valid + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// + public NativeArray penalties; + + /// + /// Tags of all nodes + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid (but if erosion uses tags then it will be overwritten later) + /// - BeforeConnections: Valid (but if erosion uses tags then it will be overwritten later) + /// - AfterConnections: Valid (but if erosion uses tags then it will be overwritten later) + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// + public NativeArray tags; + + /// + /// Normals of all nodes. + /// If height testing is disabled the normal will be (0,1,0) for all nodes. + /// If a node doesn't exist (only happens in layered grid graphs) or if the height raycast didn't hit anything then the normal will be (0,0,0). + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid + /// - BeforeConnections: Valid + /// - AfterConnections: Valid + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// + public NativeArray normals; + + /// + /// Walkability of all nodes before erosion happens. + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid (it will be combined with collision testing later) + /// - BeforeConnections: Valid + /// - AfterConnections: Valid + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// + public NativeArray walkable; + + /// + /// Walkability of all nodes after erosion happens. This is the final walkability of the nodes. + /// If no erosion is used then the data will just be copied from the array. + /// + /// Data is valid in these passes: + /// - BeforeCollision: Invalid + /// - BeforeConnections: Invalid + /// - AfterConnections: Invalid + /// - AfterErosion: Valid + /// - PostProcess: Valid + /// + public NativeArray walkableWithErosion; + + + /// + /// True if the data may have multiple layers. + /// For layered data the nodes are laid out as `data[y*width*depth + z*width + x]`. + /// For non-layered data the nodes are laid out as `data[z*width + x]` (which is equivalent to the above layout assuming y=0). + /// + /// This also affects how node connections are stored. You can use and to access + /// connections for the different data layouts. + /// + public bool layeredDataLayout; + + public void AllocateBuffers (JobDependencyTracker dependencyTracker) { + Profiler.BeginSample("Allocating buffers"); + // Allocate buffers for jobs + // Allocating buffers with uninitialized memory is much faster if no jobs assume anything about their contents + if (dependencyTracker != null) { + positions = dependencyTracker.NewNativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + normals = dependencyTracker.NewNativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + connections = dependencyTracker.NewNativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + penalties = dependencyTracker.NewNativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + walkable = dependencyTracker.NewNativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + walkableWithErosion = dependencyTracker.NewNativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + tags = dependencyTracker.NewNativeArray(numNodes, allocationMethod, NativeArrayOptions.ClearMemory); + } else { + positions = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + normals = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + connections = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + penalties = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + walkable = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + walkableWithErosion = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + tags = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.ClearMemory); + } + Profiler.EndSample(); + } + + public void TrackBuffers (JobDependencyTracker dependencyTracker) { + if (positions.IsCreated) dependencyTracker.Track(positions); + if (normals.IsCreated) dependencyTracker.Track(normals); + if (connections.IsCreated) dependencyTracker.Track(connections); + if (penalties.IsCreated) dependencyTracker.Track(penalties); + if (walkable.IsCreated) dependencyTracker.Track(walkable); + if (walkableWithErosion.IsCreated) dependencyTracker.Track(walkableWithErosion); + if (tags.IsCreated) dependencyTracker.Track(tags); + } + + public void PersistBuffers (JobDependencyTracker dependencyTracker) { + dependencyTracker.Persist(positions); + dependencyTracker.Persist(normals); + dependencyTracker.Persist(connections); + dependencyTracker.Persist(penalties); + dependencyTracker.Persist(walkable); + dependencyTracker.Persist(walkableWithErosion); + dependencyTracker.Persist(tags); + } + + public void Dispose () { + bounds = default; + numNodes = 0; + if (positions.IsCreated) positions.Dispose(); + if (normals.IsCreated) normals.Dispose(); + if (connections.IsCreated) connections.Dispose(); + if (penalties.IsCreated) penalties.Dispose(); + if (walkable.IsCreated) walkable.Dispose(); + if (walkableWithErosion.IsCreated) walkableWithErosion.Dispose(); + if (tags.IsCreated) tags.Dispose(); + } + + public JobHandle Rotate2D (int dx, int dz, JobHandle dependency) { + var size = bounds.size; + unsafe { + var jobs = stackalloc JobHandle[7]; + jobs[0] = positions.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[1] = normals.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[2] = connections.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[3] = penalties.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[4] = walkable.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[5] = walkableWithErosion.Rotate3D(size, dx, dz).Schedule(dependency); + jobs[6] = tags.Rotate3D(size, dx, dz).Schedule(dependency); + return JobHandleUnsafeUtility.CombineDependencies(jobs, 7); + } + } + + public void ResizeLayerCount (int layerCount, JobDependencyTracker dependencyTracker) { + if (layerCount > layers) { + var oldData = this; + this.bounds.max.y = layerCount; + this.numNodes = bounds.volume; + this.AllocateBuffers(dependencyTracker); + // Ensure the normals for the upper layers are zeroed out. + // All other node data in the upper layers can be left uninitialized. + this.normals.MemSet(float4.zero).Schedule(dependencyTracker); + this.walkable.MemSet(false).Schedule(dependencyTracker); + this.walkableWithErosion.MemSet(false).Schedule(dependencyTracker); + new JobCopyBuffers { + input = oldData, + output = this, + copyPenaltyAndTags = true, + bounds = oldData.bounds, + }.Schedule(dependencyTracker); + } + if (layerCount < layers) { + throw new System.ArgumentException("Cannot reduce the number of layers"); + } + } + + struct LightReader : GridIterationUtilities.ISliceAction { + public GridNodeBase[] nodes; + public UnsafeSpan nodePositions; + public UnsafeSpan nodeWalkable; + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public void Execute (uint outerIdx, uint innerIdx) { + // The data bounds may have more layers than the existing nodes if a new layer is being added. + // We can only copy from the nodes that exist. + if (outerIdx < nodes.Length) { + var node = nodes[outerIdx]; + if (node != null) { + nodePositions[innerIdx] = (Vector3)node.position; + nodeWalkable[innerIdx] = node.Walkable; + return; + } + } + + // Fallback in case the node was null (only happens for layered grid graphs), + // or if we are adding more layers to the graph, in which case we are outside + // the bounds of the nodes array. + nodePositions[innerIdx] = Vector3.zero; + nodeWalkable[innerIdx] = false; + } + } + + public void ReadFromNodesForConnectionCalculations (GridNodeBase[] nodes, Slice3D slice, JobHandle nodesDependsOn, NativeArray graphNodeNormals, JobDependencyTracker dependencyTracker) { + bounds = slice.slice; + numNodes = slice.slice.volume; + + Profiler.BeginSample("Allocating buffers"); + positions = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + normals = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + connections = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + walkableWithErosion = new NativeArray(numNodes, allocationMethod, NativeArrayOptions.UninitializedMemory); + Profiler.EndSample(); + + Profiler.BeginSample("Reading node data"); + var reader = new LightReader { + nodes = nodes, + nodePositions = this.positions.AsUnsafeSpan(), + nodeWalkable = this.walkableWithErosion.AsUnsafeSpan(), + }; + GridIterationUtilities.ForEachCellIn3DSlice(slice, ref reader); + Profiler.EndSample(); + + ReadNodeNormals(slice, graphNodeNormals, dependencyTracker); + } + + void ReadNodeNormals (Slice3D slice, NativeArray graphNodeNormals, JobDependencyTracker dependencyTracker) { + UnityEngine.Assertions.Assert.IsTrue(graphNodeNormals.IsCreated); + // Read the normal data from the graphNodeNormals array and copy it to the nodeNormals array. + // The nodeArrayBounds may have fewer layers than the readBounds if layers are being added. + // This means we can copy only a subset of the normals. + // We MemSet the array to zero first to avoid any uninitialized data remaining. + // TODO: Do clamping in caller + //var clampedReadBounds = new IntBounds(readBounds.min, new int3(readBounds.max.x, math.min(nodeArrayBounds.y, readBounds.max.y), readBounds.max.z)); + if (dependencyTracker != null) { + normals.MemSet(float4.zero).Schedule(dependencyTracker); + new JobCopyRectangle { + input = graphNodeNormals, + output = normals, + inputSlice = slice, + outputSlice = new Slice3D(bounds, slice.slice), + }.Schedule(dependencyTracker); + } else { + Profiler.BeginSample("ReadNodeNormals"); + normals.AsUnsafeSpan().FillZeros(); + JobCopyRectangle.Copy(graphNodeNormals, normals, slice, new Slice3D(bounds, slice.slice)); + Profiler.EndSample(); + } + } + + public static GridGraphNodeData ReadFromNodes (GridNodeBase[] nodes, Slice3D slice, JobHandle nodesDependsOn, NativeArray graphNodeNormals, Allocator allocator, bool layeredDataLayout, JobDependencyTracker dependencyTracker) { + var nodeData = new GridGraphNodeData { + allocationMethod = allocator, + numNodes = slice.slice.volume, + bounds = slice.slice, + layeredDataLayout = layeredDataLayout, + }; + nodeData.AllocateBuffers(dependencyTracker); + + // This is a managed type, we need to trick Unity to allow this inside of a job + var nodesHandle = System.Runtime.InteropServices.GCHandle.Alloc(nodes); + + var job = new JobReadNodeData { + nodesHandle = nodesHandle, + nodePositions = nodeData.positions, + nodePenalties = nodeData.penalties, + nodeTags = nodeData.tags, + nodeConnections = nodeData.connections, + nodeWalkableWithErosion = nodeData.walkableWithErosion, + nodeWalkable = nodeData.walkable, + slice = slice, + }.ScheduleBatch(nodeData.numNodes, math.max(2000, nodeData.numNodes/16), dependencyTracker, nodesDependsOn); + + dependencyTracker.DeferFree(nodesHandle, job); + + if (graphNodeNormals.IsCreated) nodeData.ReadNodeNormals(slice, graphNodeNormals, dependencyTracker); + return nodeData; + } + + public GridGraphNodeData ReadFromNodesAndCopy (GridNodeBase[] nodes, Slice3D slice, JobHandle nodesDependsOn, NativeArray graphNodeNormals, bool copyPenaltyAndTags, JobDependencyTracker dependencyTracker) { + var newData = GridGraphNodeData.ReadFromNodes(nodes, slice, nodesDependsOn, graphNodeNormals, allocationMethod, layeredDataLayout, dependencyTracker); + // Overwrite a rectangle in the center with the data from this object. + // In the end we will have newly calculated data in the middle and data read from nodes along the borders + newData.CopyFrom(this, copyPenaltyAndTags, dependencyTracker); + return newData; + } + + public void CopyFrom(GridGraphNodeData other, bool copyPenaltyAndTags, JobDependencyTracker dependencyTracker) => CopyFrom(other, IntBounds.Intersection(bounds, other.bounds), copyPenaltyAndTags, dependencyTracker); + + public void CopyFrom (GridGraphNodeData other, IntBounds bounds, bool copyPenaltyAndTags, JobDependencyTracker dependencyTracker) { + var job = new JobCopyBuffers { + input = other, + output = this, + copyPenaltyAndTags = copyPenaltyAndTags, + bounds = bounds, + }; + if (dependencyTracker != null) { + job.Schedule(dependencyTracker); + } else { +#if UNITY_2022_2_OR_NEWER + job.RunByRef(); +#else + job.Run(); +#endif + } + } + + public JobHandle AssignToNodes (GridNodeBase[] nodes, int3 nodeArrayBounds, IntBounds writeMask, uint graphIndex, JobHandle nodesDependsOn, JobDependencyTracker dependencyTracker) { + // This is a managed type, we need to trick Unity to allow this inside of a job + var nodesHandle = System.Runtime.InteropServices.GCHandle.Alloc(nodes); + + // Assign the data to the nodes (in parallel for performance) + // This will also dirty all nodes, but that is a thread-safe operation. + var job2 = new JobWriteNodeData { + nodesHandle = nodesHandle, + graphIndex = graphIndex, + nodePositions = positions, + nodePenalties = penalties, + nodeTags = tags, + nodeConnections = connections, + nodeWalkableWithErosion = walkableWithErosion, + nodeWalkable = walkable, + nodeArrayBounds = nodeArrayBounds, + dataBounds = bounds, + writeMask = writeMask, + }.ScheduleBatch(writeMask.volume, math.max(1000, writeMask.volume/16), dependencyTracker, nodesDependsOn); + + dependencyTracker.DeferFree(nodesHandle, job2); + return job2; + } + } + + public struct GridGraphScanData { + /// + /// Tracks dependencies between jobs to allow parallelism without tediously specifying dependencies manually. + /// Always use when scheduling jobs. + /// + public JobDependencyTracker dependencyTracker; + + /// The up direction of the graph, in world space + public Vector3 up; + + /// Transforms graph-space to world space + public GraphTransform transform; + + /// Data for all nodes in the graph update that is being calculated + public GridGraphNodeData nodes; + + /// + /// Bounds of the data arrays. + /// Deprecated: Use nodes.bounds or heightHitsBounds depending on if you are using the heightHits array or not + /// + [System.Obsolete("Use nodes.bounds or heightHitsBounds depending on if you are using the heightHits array or not")] + public IntBounds bounds => nodes.bounds; + + /// + /// True if the data may have multiple layers. + /// For layered data the nodes are laid out as `data[y*width*depth + z*width + x]`. + /// For non-layered data the nodes are laid out as `data[z*width + x]` (which is equivalent to the above layout assuming y=0). + /// + /// Deprecated: Use nodes.layeredDataLayout instead + /// + [System.Obsolete("Use nodes.layeredDataLayout instead")] + public bool layeredDataLayout => nodes.layeredDataLayout; + + /// + /// Raycasts hits used for height testing. + /// This data is only valid if height testing is enabled, otherwise the array is uninitialized (heightHits.IsCreated will be false). + /// + /// Data is valid in these passes: + /// - BeforeCollision: Valid (if height testing is enabled) + /// - BeforeConnections: Valid (if height testing is enabled) + /// - AfterConnections: Valid (if height testing is enabled) + /// - AfterErosion: Valid (if height testing is enabled) + /// - PostProcess: Valid (if height testing is enabled) + /// + /// Warning: This array does not have the same size as the arrays in . It will usually be slightly smaller. See . + /// + public NativeArray heightHits; + + /// + /// Bounds for the array. + /// + /// During an update, the scan data may contain more nodes than we are doing height testing for. + /// For a few nodes around the update, the data will be read from the existing graph, instead. This is done for performance. + /// This means that there may not be any height testing information these nodes. + /// However, all nodes that will be written to will always have height testing information. + /// + public IntBounds heightHitsBounds; + + /// + /// Node positions. + /// Deprecated: Use instead + /// + [System.Obsolete("Use nodes.positions instead")] + public NativeArray nodePositions => nodes.positions; + + /// + /// Node connections. + /// Deprecated: Use instead + /// + [System.Obsolete("Use nodes.connections instead")] + public NativeArray nodeConnections => nodes.connections; + + /// + /// Node penalties. + /// Deprecated: Use instead + /// + [System.Obsolete("Use nodes.penalties instead")] + public NativeArray nodePenalties => nodes.penalties; + + /// + /// Node tags. + /// Deprecated: Use instead + /// + [System.Obsolete("Use nodes.tags instead")] + public NativeArray nodeTags => nodes.tags; + + /// + /// Node normals. + /// Deprecated: Use instead + /// + [System.Obsolete("Use nodes.normals instead")] + public NativeArray nodeNormals => nodes.normals; + + /// + /// Node walkability. + /// Deprecated: Use instead + /// + [System.Obsolete("Use nodes.walkable instead")] + public NativeArray nodeWalkable => nodes.walkable; + + /// + /// Node walkability with erosion. + /// Deprecated: Use instead + /// + [System.Obsolete("Use nodes.walkableWithErosion instead")] + public NativeArray nodeWalkableWithErosion => nodes.walkableWithErosion; + + public void SetDefaultPenalties (uint initialPenalty) { + nodes.penalties.MemSet(initialPenalty).Schedule(dependencyTracker); + } + + public void SetDefaultNodePositions (GraphTransform transform) { + new JobNodeGridLayout { + graphToWorld = transform.matrix, + bounds = nodes.bounds, + nodePositions = nodes.positions, + }.Schedule(dependencyTracker); + } + + public JobHandle HeightCheck (GraphCollision collision, int maxHits, IntBounds recalculationBounds, NativeArray outLayerCount, float characterHeight, Allocator allocator) { + // For some reason the physics code crashes when allocating raycastCommands with UninitializedMemory, even though I have verified that every + // element in the array is set to a well defined value before the physics code gets to it... Mysterious. + var cellCount = recalculationBounds.size.x * recalculationBounds.size.z; + var raycastCommands = dependencyTracker.NewNativeArray(cellCount, allocator, NativeArrayOptions.ClearMemory); + + heightHits = dependencyTracker.NewNativeArray(cellCount * maxHits, allocator, NativeArrayOptions.ClearMemory); + heightHitsBounds = recalculationBounds; + + // Due to floating point inaccuracies we don't want the rays to end *exactly* at the base of the graph + // The rays may or may not hit colliders with the exact same y coordinate. + // We extend the rays a bit to ensure they always hit + const float RayLengthMargin = 0.01f; + var prepareJob = new JobPrepareGridRaycast { + graphToWorld = transform.matrix, + bounds = recalculationBounds, + physicsScene = Physics.defaultPhysicsScene, + raycastOffset = up * collision.fromHeight, + raycastDirection = -up * (collision.fromHeight + RayLengthMargin), + raycastMask = collision.heightMask, + raycastCommands = raycastCommands, + }.Schedule(dependencyTracker); + + if (maxHits > 1) { + // Skip this distance between each hit. + // It is pretty arbitrarily chosen, but it must be lower than characterHeight. + // If it would be set too low then many thin colliders stacked on top of each other could lead to a very large number of hits + // that will not lead to any walkable nodes anyway. + float minStep = characterHeight * 0.5f; + var dependency = new JobRaycastAll(raycastCommands, heightHits, Physics.defaultPhysicsScene, maxHits, allocator, dependencyTracker, minStep).Schedule(prepareJob); + + dependency = new JobMaxHitCount { + hits = heightHits, + maxHits = maxHits, + layerStride = cellCount, + maxHitCount = outLayerCount, + }.Schedule(dependency); + + return dependency; + } else { + dependencyTracker.ScheduleBatch(raycastCommands, heightHits, 2048); + outLayerCount[0] = 1; + return default; + } + } + + public void CopyHits (IntBounds recalculationBounds) { + // Copy the hit points and normals to separate arrays + // Ensure the normals for the upper layers are zeroed out. + nodes.normals.MemSet(float4.zero).Schedule(dependencyTracker); + new JobCopyHits { + hits = heightHits, + points = nodes.positions, + normals = nodes.normals, + slice = new Slice3D(nodes.bounds, recalculationBounds), + }.Schedule(dependencyTracker); + } + + public void CalculateWalkabilityFromHeightData (bool useRaycastNormal, bool unwalkableWhenNoGround, float maxSlope, float characterHeight) { + new JobNodeWalkability { + useRaycastNormal = useRaycastNormal, + unwalkableWhenNoGround = unwalkableWhenNoGround, + maxSlope = maxSlope, + up = up, + nodeNormals = nodes.normals, + nodeWalkable = nodes.walkable, + nodePositions = nodes.positions.Reinterpret(), + characterHeight = characterHeight, + layerStride = nodes.bounds.size.x*nodes.bounds.size.z, + }.Schedule(dependencyTracker); + } + + public IEnumerator CollisionCheck (GraphCollision collision, IntBounds calculationBounds) { + if (collision.type == ColliderType.Ray && !collision.use2D) { + var collisionCheckResult = dependencyTracker.NewNativeArray(nodes.numNodes, nodes.allocationMethod, NativeArrayOptions.UninitializedMemory); + collision.JobCollisionRay(nodes.positions, collisionCheckResult, up, nodes.allocationMethod, dependencyTracker); + nodes.walkable.BitwiseAndWith(collisionCheckResult).WithLength(nodes.numNodes).Schedule(dependencyTracker); + return null; + +// Before Unity 2023.3, these features compile, but they will cause memory corruption in some cases, due to a bug in Unity +#if UNITY_2022_2_OR_NEWER && UNITY_2023_3_OR_NEWER && UNITY_HAS_FIXED_MEMORY_CORRUPTION_ISSUE + } else if (collision.type == ColliderType.Capsule && !collision.use2D) { + var collisionCheckResult = dependencyTracker.NewNativeArray(nodes.numNodes, nodes.allocationMethod, NativeArrayOptions.UninitializedMemory); + collision.JobCollisionCapsule(nodes.positions, collisionCheckResult, up, nodes.allocationMethod, dependencyTracker); + nodes.walkable.BitwiseAndWith(collisionCheckResult).WithLength(nodes.numNodes).Schedule(dependencyTracker); + return null; + } else if (collision.type == ColliderType.Sphere && !collision.use2D) { + var collisionCheckResult = dependencyTracker.NewNativeArray(nodes.numNodes, nodes.allocationMethod, NativeArrayOptions.UninitializedMemory); + collision.JobCollisionSphere(nodes.positions, collisionCheckResult, up, nodes.allocationMethod, dependencyTracker); + nodes.walkable.BitwiseAndWith(collisionCheckResult).WithLength(nodes.numNodes).Schedule(dependencyTracker); + return null; +#endif + } else { + // This part can unfortunately not be jobified yet + return new JobCheckCollisions { + nodePositions = nodes.positions, + collisionResult = nodes.walkable, + collision = collision, + }.ExecuteMainThreadJob(dependencyTracker); + } + } + + public void Connections (float maxStepHeight, bool maxStepUsesSlope, IntBounds calculationBounds, NumNeighbours neighbours, bool cutCorners, bool use2D, bool useErodedWalkability, float characterHeight) { + var job = new JobCalculateGridConnections { + maxStepHeight = maxStepHeight, + maxStepUsesSlope = maxStepUsesSlope, + up = up, + bounds = calculationBounds.Offset(-nodes.bounds.min), + arrayBounds = nodes.bounds.size, + neighbours = neighbours, + use2D = use2D, + cutCorners = cutCorners, + nodeWalkable = (useErodedWalkability ? nodes.walkableWithErosion : nodes.walkable).AsUnsafeSpanNoChecks(), + nodePositions = nodes.positions.AsUnsafeSpanNoChecks(), + nodeNormals = nodes.normals.AsUnsafeSpanNoChecks(), + nodeConnections = nodes.connections.AsUnsafeSpanNoChecks(), + characterHeight = characterHeight, + layeredDataLayout = nodes.layeredDataLayout, + }; + + if (dependencyTracker != null) { + job.ScheduleBatch(calculationBounds.size.z, 20, dependencyTracker); + } else { + job.RunBatch(calculationBounds.size.z); + } + + // For single layer graphs this will have already been done in the JobCalculateGridConnections job + // but for layered grid graphs we need to handle things differently because the data layout is different. + // It needs to be done after all axis aligned connections have been calculated. + if (nodes.layeredDataLayout) { + var job2 = new JobFilterDiagonalConnections { + slice = new Slice3D(nodes.bounds, calculationBounds), + neighbours = neighbours, + cutCorners = cutCorners, + nodeConnections = nodes.connections.AsUnsafeSpanNoChecks(), + }; + if (dependencyTracker != null) { + job2.ScheduleBatch(calculationBounds.size.z, 20, dependencyTracker); + } else { + job2.RunBatch(calculationBounds.size.z); + } + } + } + + public void Erosion (NumNeighbours neighbours, int erodeIterations, IntBounds erosionWriteMask, bool erosionUsesTags, int erosionStartTag, int erosionTagsPrecedenceMask) { + if (!nodes.layeredDataLayout) { + new JobErosion { + bounds = nodes.bounds, + writeMask = erosionWriteMask, + neighbours = neighbours, + nodeConnections = nodes.connections, + erosion = erodeIterations, + nodeWalkable = nodes.walkable, + outNodeWalkable = nodes.walkableWithErosion, + nodeTags = nodes.tags, + erosionUsesTags = erosionUsesTags, + erosionStartTag = erosionStartTag, + erosionTagsPrecedenceMask = erosionTagsPrecedenceMask, + }.Schedule(dependencyTracker); + } else { + new JobErosion { + bounds = nodes.bounds, + writeMask = erosionWriteMask, + neighbours = neighbours, + nodeConnections = nodes.connections, + erosion = erodeIterations, + nodeWalkable = nodes.walkable, + outNodeWalkable = nodes.walkableWithErosion, + nodeTags = nodes.tags, + erosionUsesTags = erosionUsesTags, + erosionStartTag = erosionStartTag, + erosionTagsPrecedenceMask = erosionTagsPrecedenceMask, + }.Schedule(dependencyTracker); + } + } + + public void AssignNodeConnections (GridNodeBase[] nodes, int3 nodeArrayBounds, IntBounds writeBounds) { + var bounds = this.nodes.bounds; + var writeDataOffset = writeBounds.min - bounds.min; + var nodeConnections = this.nodes.connections.AsUnsafeReadOnlySpan(); + for (int y = 0; y < writeBounds.size.y; y++) { + var yoffset = (y + writeBounds.min.y)*nodeArrayBounds.x*nodeArrayBounds.z; + for (int z = 0; z < writeBounds.size.z; z++) { + var zoffset = yoffset + (z + writeBounds.min.z)*nodeArrayBounds.x + writeBounds.min.x; + var zoffset2 = (y+writeDataOffset.y)*bounds.size.x*bounds.size.z + (z+writeDataOffset.z)*bounds.size.x + writeDataOffset.x; + for (int x = 0; x < writeBounds.size.x; x++) { + var node = nodes[zoffset + x]; + var dataIdx = zoffset2 + x; + var conn = nodeConnections[dataIdx]; + + if (node == null) continue; + + if (node is LevelGridNode lgnode) { + lgnode.SetAllConnectionInternal(conn); + } else { + var gnode = node as GridNode; + gnode.SetAllConnectionInternal((int)conn); + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs.meta new file mode 100644 index 0000000..c108af9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridGraphScanData.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: cd8d2fda4c637484c806417e77602960 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs new file mode 100644 index 0000000..8d5485b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs @@ -0,0 +1,261 @@ +using Pathfinding.Jobs; +using Unity.Collections; +using Unity.Mathematics; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid { + /// + /// Helpers for iterating over grid graph data. + /// + /// This is a helper for iterating over grid graph data, which is typically stored in an array of size width*layers*depth (x*y*z). + /// It is used internally by grid graph jobs, and can also be used by custom grid graph rules. + /// + /// See: grid-rules-write (view in online documentation for working links) + /// + public static class GridIterationUtilities { + /// Callback struct for + public interface ISliceAction { + void Execute(uint outerIdx, uint innerIdx); + } + + /// Callback struct for + public interface ISliceActionWithCoords { + void Execute(uint outerIdx, uint innerIdx, int3 innerCoords); + } + + /// Callback struct for + public interface ICellAction { + void Execute(uint idx, int x, int y, int z); + } + + /// + /// Iterates over a slice of a 3D array. + /// + /// This is a helper for iterating over grid graph data, which is typically stored in an array of size width*layers*depth (x*y*z). + /// + /// In burst-compiled code, this will be essentially as fast as writing the loop code yourself. In C#, it is marginally slower than writing the loop code yourself. + /// + /// [Open online documentation to see images] + /// + /// Bounds of the slice and the size of the outer array it is relative to. + /// Your callback struct. The Execute method on the callback struct will be called for each element in the slice. It will be passed both the index in the slice and the index in the outer array. + public static void ForEachCellIn3DSlice(Slice3D slice, ref T action) where T : struct, ISliceAction { + var size = slice.slice.size; + var(strideX, strideY, strideZ) = slice.outerStrides; + var outerOffset = slice.outerStartIndex; + uint i = 0; + for (int y = 0; y < size.y; y++) { + for (int z = 0; z < size.z; z++) { + int offset2 = y*strideY + z*strideZ + outerOffset; + for (int x = 0; x < size.x; x++, i++) { + action.Execute((uint)(offset2 + x), i); + } + } + } + } + + /// + /// Iterates over a slice of a 3D array. + /// + /// This is a helper for iterating over grid graph data, which is typically stored in an array of size width*layers*depth (x*y*z). + /// + /// In burst-compiled code, this will be essentially as fast as writing the loop code yourself. In C#, it is marginally slower than writing the loop code yourself. + /// + /// [Open online documentation to see images] + /// + /// Bounds of the slice and the size of the outer array it is relative to. + /// Your callback struct. The Execute method on the callback struct will be called for each element in the slice. It will be passed both the index in the slice and the index in the outer array. + public static void ForEachCellIn3DSliceWithCoords(Slice3D slice, ref T action) where T : struct, ISliceActionWithCoords { + var size = slice.slice.size; + var(strideX, strideY, strideZ) = slice.outerStrides; + var outerOffset = slice.outerStartIndex; + uint i = (uint)(size.x*size.y*size.z) - 1; + for (int y = size.y - 1; y >= 0; y--) { + for (int z = size.z - 1; z >= 0; z--) { + int offset2 = y*strideY + z*strideZ + outerOffset; + for (int x = size.x - 1; x >= 0; x--, i--) { + action.Execute((uint)(offset2 + x), i, new int3(x, y, z)); + } + } + } + } + + /// + /// Iterates over a 3D array. + /// + /// This is a helper for iterating over grid graph data, which is typically stored in an array of size width*layers*depth (x*y*z). + /// + /// In burst-compiled code, this will be essentially as fast as writing the loop code yourself. In C#, it is marginally slower than writing the loop code yourself. + /// + /// Size of the array. + /// Your callback struct. The Execute method on the callback struct will be called for each element in the array. It will be passed the x, y and z coordinates of the element as well as the index in the array. + public static void ForEachCellIn3DArray(int3 size, ref T action) where T : struct, ICellAction { + uint i = (uint)(size.x*size.y*size.z) - 1; + for (int y = size.y - 1; y >= 0; y--) { + for (int z = size.z - 1; z >= 0; z--) { + for (int x = size.x - 1; x >= 0; x--, i--) { + action.Execute(i, x, y, z); + } + } + } + } + + /// + /// Helper interface for modifying nodes. + /// This is used by the function. + /// + public interface INodeModifier { + /// + /// Called for every node that is being updated. + /// + /// See: gridgraphrule-burst (view in online documentation for working links) for example usage. + /// + /// Index of the node. This is the index in the data arrays for the graph update, not necessarily the index in the graph. + /// X coordinate of the node, relative to the updated region. + /// Layer (Y) coordinate of the node, relative to the updated region. + /// Z coordinate of the node, relative to the updated region. + void ModifyNode(int dataIndex, int dataX, int dataLayer, int dataZ); + } + + /// + /// Iterate through all nodes that exist. + /// + /// See: grid-rules-write (view in online documentation for working links) for example usage. + /// + /// Size of the rectangle of the grid graph that is being updated/scanned + /// Data for all node normals. This is used to determine if a node exists (important for layered grid graphs). + /// The ModifyNode method on the callback struct will be called for each node. + public static void ForEachNode(int3 arrayBounds, NativeArray nodeNormals, ref T callback) where T : struct, INodeModifier { + Assert.IsTrue(nodeNormals.Length == arrayBounds.x * arrayBounds.y * arrayBounds.z); + int i = 0; + + for (int y = 0; y < arrayBounds.y; y++) { + for (int z = 0; z < arrayBounds.z; z++) { + for (int x = 0; x < arrayBounds.x; x++, i++) { + // Check if the node exists at all + // This is important for layered grid graphs + // A normal is never zero otherwise + if (math.any(nodeNormals[i])) { + callback.ModifyNode(i, x, y, z); + } + } + } + } + } + + /// + /// Helper interface for modifying node connections. + /// This is used by the function. + /// + public interface IConnectionFilter { + /// + /// Returns true if the connection should be enabled. + /// + /// See: gridgraphrule-connection-filter (view in online documentation for working links) for example usage. + /// See: + /// + /// Index of the node for which the connection is being tested. This is the index in the data arrays for the graph update, not necessarily the index in the graph. + /// X coordinate of the node for which the connection is being tested, relative to the updated region. + /// Layer (Y) coordinate of the node for which the connection is being tested, relative to the updated region. + /// Z coordinate of the node for which the connection is being tested, relative to the updated region. + /// Direction to the neighbour. See \reflink{GridNode.HasConnectionInDirection}. + /// Index of the neighbour node. This is the index in the data arrays for the graph update, not necessarily the index in the graph. + bool IsValidConnection(int dataIndex, int dataX, int dataLayer, int dataZ, int direction, int neighbourDataIndex); + } + + /// + /// Iterate through all enabled connections of all nodes. + /// + /// See: grid-rules-write (view in online documentation for working links) for example usage. + /// + /// Sub-rectangle of the grid graph that is being updated/scanned + /// Data with all node connections. + /// Should be true for layered grid graphs and false otherwise. + /// Your callback struct. The IsValidConnection method on the callback struct will be called for each connection. If false is returned, the connection will be disabled. + public static void FilterNodeConnections(IntBounds bounds, NativeArray nodeConnections, bool layeredDataLayout, ref T filter) where T : struct, IConnectionFilter { + var size = bounds.size; + Assert.IsTrue(nodeConnections.Length == size.x * size.y * size.z); + unsafe { + var neighbourOffsets = stackalloc int[8]; + for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * size.x + GridGraph.neighbourXOffsets[i]; + var layerStride = size.x * size.z; + + int nodeIndex = 0; + for (int y = 0; y < size.y; y++) { + for (int z = 0; z < size.z; z++) { + for (int x = 0; x < size.x; x++, nodeIndex++) { + var conn = nodeConnections[nodeIndex]; + if (layeredDataLayout) { + // Layered grid graph + for (int dir = 0; dir < 8; dir++) { + var connectionValue = (int)((conn >> LevelGridNode.ConnectionStride*dir) & LevelGridNode.ConnectionMask); + if (connectionValue != LevelGridNode.NoConnection && !filter.IsValidConnection(nodeIndex, x, y, z, dir, nodeIndex + neighbourOffsets[dir] + (connectionValue - y)*layerStride)) { + conn |= (ulong)LevelGridNode.NoConnection << LevelGridNode.ConnectionStride*dir; + } + } + } else { + // Normal grid graph + // Iterate through all connections on the node + for (int dir = 0; dir < 8; dir++) { + if (((int)conn & (1 << dir)) != 0 && !filter.IsValidConnection(nodeIndex, x, y, z, dir, nodeIndex + neighbourOffsets[dir])) { + conn &= ~(1UL << dir); + } + } + } + nodeConnections[nodeIndex] = conn; + } + } + } + } + } + + /// + /// Returns the data index for a node's neighbour in the given direction. + /// + /// The bounds, nodeConnections and layeredDataLayout fields can be retrieved from the .data object. + /// + /// Returns: Null if the node has no connection in that direction. Otherwise the data index for that node is returned. + /// + /// See: gridgraphrule-connection-filter (view in online documentation for working links) for example usage. + /// + /// Sub-rectangle of the grid graph that is being updated/scanned + /// Data for all node connections + /// True if this is a layered grid graph + /// X coordinate in the data arrays for the node for which you want to get a neighbour + /// Layer (Y) coordinate in the data arrays for the node for which you want to get a neighbour + /// Z coordinate in the data arrays for the node for which you want to get a neighbour + /// Direction to the neighbour. See \reflink{GridNode.HasConnectionInDirection}. + public static int? GetNeighbourDataIndex (IntBounds bounds, NativeArray nodeConnections, bool layeredDataLayout, int dataX, int dataLayer, int dataZ, int direction) { + // Find the coordinates of the adjacent node + var dx = GridGraph.neighbourXOffsets[direction]; + var dz = GridGraph.neighbourZOffsets[direction]; + + int nx = dataX + dx; + int nz = dataZ + dz; + + // The data arrays are laid out row by row + const int xstride = 1; + var zstride = bounds.size.x; + var ystride = bounds.size.x * bounds.size.z; + + var dataIndex = dataLayer * ystride + dataZ * zstride + dataX * xstride; + var neighbourDataIndex = nz * zstride + nx * xstride; + + if (layeredDataLayout) { + // In a layered grid graph we need to account for nodes in different layers + var ny = (nodeConnections[dataIndex] >> LevelGridNode.ConnectionStride*direction) & LevelGridNode.ConnectionMask; + if (ny == LevelGridNode.NoConnection) return null; + + // For valid nodeConnections arrays this is not necessary as out of bounds connections are not valid and it will thus be caught above in the 'has connection' check. + // But let's be safe in case users do something weird + if (nx < 0 || nz < 0 || nx >= bounds.size.x || nz >= bounds.size.z) throw new System.Exception("Node has an invalid connection to a node outside the bounds of the graph"); + + neighbourDataIndex += (int)ny * ystride; + } else + if ((nodeConnections[dataIndex] & (1UL << direction)) == 0) return null; + + if (nx < 0 || nz < 0 || nx >= bounds.size.x || nz >= bounds.size.z) throw new System.Exception("Node has an invalid connection to a node outside the bounds of the graph"); + return neighbourDataIndex; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs.meta new file mode 100644 index 0000000..ba19ff9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/GridIterationUtilities.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0a7d565c3874ce349a83c260182a8b63 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs.meta new file mode 100644 index 0000000..6cc1028 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: ed5b7a8f175d0794db354a0757bb79ec +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs new file mode 100644 index 0000000..d301d44 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs @@ -0,0 +1,57 @@ +using Pathfinding.Util; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Allocates and deallocates nodes in a grid graph. + /// + /// This will inspect every cell in the dataBounds and allocate or deallocate the node depending on if that slot should have a node or not according to the nodeNormals array (pure zeroes means no node). + /// + /// This is only used for incremental updates of grid graphs. + /// The initial layer of the grid graph (which is always filled with nodes) is allocated in the method. + /// + public struct JobAllocateNodes : IJob { + public AstarPath active; + [ReadOnly] + public NativeArray nodeNormals; + public IntBounds dataBounds; + public int3 nodeArrayBounds; + public GridNodeBase[] nodes; + public System.Func newGridNodeDelegate; + + public void Execute () { + var size = dataBounds.size; + + // Start at y=1 because all nodes at y=0 are guaranteed to already be allocated (they are always allocated in a layered grid graph). + var nodeNormalsSpan = nodeNormals.AsUnsafeReadOnlySpan(); + for (int y = 1; y < size.y; y++) { + for (int z = 0; z < size.z; z++) { + var rowOffset = ((y + dataBounds.min.y) * nodeArrayBounds.z + (z + dataBounds.min.z)) * nodeArrayBounds.x + dataBounds.min.x; + for (int x = 0; x < size.x; x++) { + var nodeIndex = rowOffset + x; + var shouldHaveNode = math.any(nodeNormalsSpan[nodeIndex]); + var node = nodes[nodeIndex]; + var hasNode = node != null; + if (shouldHaveNode != hasNode) { + if (shouldHaveNode) { + node = nodes[nodeIndex] = newGridNodeDelegate(); + active.InitializeNode(node); + } else { + // Clear custom connections first and clear connections from other nodes to this one + node.ClearCustomConnections(true); + // Clear grid connections without clearing the connections from other nodes to this one (a bit slow) + // Since this is inside a graph update we guarantee that the grid connections will be correct at the end + // of the update anyway + node.ResetConnectionsInternal(); + node.Destroy(); + nodes[nodeIndex] = null; + } + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs.meta new file mode 100644 index 0000000..a2391b4 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobAllocateNodes.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 773a80d74b04a904faae186478d396c4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs new file mode 100644 index 0000000..0a14596 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs @@ -0,0 +1,219 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Collections.LowLevel.Unsafe; +using Unity.Jobs; +using Unity.Mathematics; +using Pathfinding.Jobs; +using Pathfinding.Util; +using System.Data; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Calculates the grid connections for all nodes. + /// + /// This is a IJobParallelForBatch job. Calculating the connections in multiple threads is faster, + /// but due to hyperthreading (used on most intel processors) the individual threads will become slower. + /// It is still worth it though. + /// + [BurstCompile(FloatMode = FloatMode.Fast, CompileSynchronously = true)] + public struct JobCalculateGridConnections : IJobParallelForBatched { + public float maxStepHeight; + /// Normalized up direction + public Vector3 up; + public IntBounds bounds; + public int3 arrayBounds; + public NumNeighbours neighbours; + public bool use2D; + public bool cutCorners; + public bool maxStepUsesSlope; + public float characterHeight; + public bool layeredDataLayout; + + [ReadOnly] + public UnsafeSpan nodeWalkable; + + [ReadOnly] + public UnsafeSpan nodeNormals; + + [ReadOnly] + public UnsafeSpan nodePositions; + + /// All bitpacked node connections + [WriteOnly] + public UnsafeSpan nodeConnections; + + public bool allowBoundsChecks => false; + + + /// + /// Check if a connection to node B is valid. + /// Node A is assumed to be walkable already + /// + public static bool IsValidConnection (float4 nodePosA, float4 nodeNormalA, bool nodeWalkableB, float4 nodePosB, float4 nodeNormalB, bool maxStepUsesSlope, float maxStepHeight, float4 up) { + if (!nodeWalkableB) return false; + + if (!maxStepUsesSlope) { + // Check their differences along the Y coordinate (well, the up direction really. It is not necessarily the Y axis). + return math.abs(math.dot(up, nodePosB - nodePosA)) <= maxStepHeight; + } else { + float4 v = nodePosB - nodePosA; + float heightDifference = math.dot(v, up); + + // Check if the step is small enough. + // This is a fast path for the common case. + if (math.abs(heightDifference) <= maxStepHeight) return true; + + float4 v_flat = (v - heightDifference * up) * 0.5f; + + // Math! + // Calculates the approximate offset along the up direction + // that the ground will have moved at the midpoint between the + // nodes compared to the nodes' center points. + float NDotU = math.dot(nodeNormalA, up); + float offsetA = -math.dot(nodeNormalA - NDotU * up, v_flat); + + NDotU = math.dot(nodeNormalB, up); + float offsetB = math.dot(nodeNormalB - NDotU * up, v_flat); + + // Check the height difference with slopes taken into account. + // Note that since we also do the heightDifference check above we will ensure slope offsets do not increase the height difference. + // If we allowed this then some connections might not be valid near the start of steep slopes. + return math.abs(heightDifference + offsetB - offsetA) <= maxStepHeight; + } + } + + public void Execute (int start, int count) { + if (layeredDataLayout) ExecuteLayered(start, count); + else ExecuteFlat(start, count); + } + + public void ExecuteFlat (int start, int count) { + if (maxStepHeight <= 0 || use2D) maxStepHeight = float.PositiveInfinity; + + float4 up = new float4(this.up.x, this.up.y, this.up.z, 0); + + NativeArray neighbourOffsets = new NativeArray(8, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * arrayBounds.x + GridGraph.neighbourXOffsets[i]; + var nodePositions = this.nodePositions.Reinterpret(); + + // The loop is parallelized over z coordinates + start += bounds.min.z; + for (int z = start; z < start + count; z++) { + var initialConnections = 0xFF; + + // Disable connections to out-of-bounds nodes + // See GridNode.HasConnectionInDirection + if (z == 0) initialConnections &= ~((1 << 0) | (1 << 7) | (1 << 4)); + if (z == arrayBounds.z - 1) initialConnections &= ~((1 << 2) | (1 << 5) | (1 << 6)); + + for (int x = bounds.min.x; x < bounds.max.x; x++) { + int nodeIndex = z * arrayBounds.x + x; + if (!nodeWalkable[nodeIndex]) { + nodeConnections[nodeIndex] = 0; + continue; + } + + // Bitpacked connections + // bit 0 is set if connection 0 is enabled + // bit 1 is set if connection 1 is enabled etc. + int conns = initialConnections; + + // Disable connections to out-of-bounds nodes + if (x == 0) conns &= ~((1 << 3) | (1 << 6) | (1 << 7)); + if (x == arrayBounds.x - 1) conns &= ~((1 << 1) | (1 << 4) | (1 << 5)); + + float4 pos = new float4(nodePositions[nodeIndex], 0); + float4 normal = nodeNormals[nodeIndex]; + + for (int i = 0; i < 8; i++) { + int neighbourIndex = nodeIndex + neighbourOffsets[i]; + if ((conns & (1 << i)) != 0 && !IsValidConnection(pos, normal, nodeWalkable[neighbourIndex], new float4(nodePositions[neighbourIndex], 0), nodeNormals[neighbourIndex], maxStepUsesSlope, maxStepHeight, up)) { + // Enable connection i + conns &= ~(1 << i); + } + } + + nodeConnections[nodeIndex] = (ulong)GridNode.FilterDiagonalConnections(conns, neighbours, cutCorners); + } + } + } + + public void ExecuteLayered (int start, int count) { + if (maxStepHeight <= 0 || use2D) maxStepHeight = float.PositiveInfinity; + + float4 up = new float4(this.up.x, this.up.y, this.up.z, 0); + + NativeArray neighbourOffsets = new NativeArray(8, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * arrayBounds.x + GridGraph.neighbourXOffsets[i]; + + var layerStride = arrayBounds.z*arrayBounds.x; + start += bounds.min.z; + for (int y = bounds.min.y; y < bounds.max.y; y++) { + // The loop is parallelized over z coordinates + for (int z = start; z < start + count; z++) { + for (int x = bounds.min.x; x < bounds.max.x; x++) { + // Bitpacked connections + ulong conns = 0; + int nodeIndexXZ = z * arrayBounds.x + x; + int nodeIndex = nodeIndexXZ + y * layerStride; + float4 pos = new float4(nodePositions[nodeIndex], 0); + float4 normal = nodeNormals[nodeIndex]; + + if (nodeWalkable[nodeIndex]) { + var ourY = math.dot(up, pos); + + float ourHeight; + if (y == arrayBounds.y-1 || !math.any(nodeNormals[nodeIndex + layerStride])) { + ourHeight = float.PositiveInfinity; + } else { + var nodeAboveNeighbourPos = new float4(nodePositions[nodeIndex + layerStride], 0); + ourHeight = math.max(0, math.dot(up, nodeAboveNeighbourPos) - ourY); + } + + for (int i = 0; i < 8; i++) { + int nx = x + GridGraph.neighbourXOffsets[i]; + int nz = z + GridGraph.neighbourZOffsets[i]; + + // Check if the new position is inside the grid + int conn = LevelGridNode.NoConnection; + if (nx >= 0 && nz >= 0 && nx < arrayBounds.x && nz < arrayBounds.z) { + int neighbourStartIndex = nodeIndexXZ + neighbourOffsets[i]; + for (int y2 = 0; y2 < arrayBounds.y; y2++) { + var neighbourIndex = neighbourStartIndex + y2 * layerStride; + float4 nodePosB = new float4(nodePositions[neighbourIndex], 0); + var neighbourY = math.dot(up, nodePosB); + // Is there a node above this one + float neighbourHeight; + if (y2 == arrayBounds.y-1 || !math.any(nodeNormals[neighbourIndex + layerStride])) { + neighbourHeight = float.PositiveInfinity; + } else { + var nodeAboveNeighbourPos = new float4(nodePositions[neighbourIndex + layerStride], 0); + neighbourHeight = math.max(0, math.dot(up, nodeAboveNeighbourPos) - neighbourY); + } + + float bottom = math.max(neighbourY, ourY); + float top = math.min(neighbourY + neighbourHeight, ourY + ourHeight); + + float dist = top-bottom; + + if (dist >= characterHeight && IsValidConnection(pos, normal, nodeWalkable[neighbourIndex], new float4(nodePositions[neighbourIndex], 0), nodeNormals[neighbourIndex], maxStepUsesSlope, maxStepHeight, up)) { + conn = y2; + } + } + } + + conns |= (ulong)conn << LevelGridNode.ConnectionStride*i; + } + } else { + conns = LevelGridNode.AllConnectionsMask; + } + + nodeConnections[nodeIndex] = conns; + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs.meta new file mode 100644 index 0000000..089d203 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCalculateGridConnections.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 63dd791a75e95424ea05940b4c155c25 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs new file mode 100644 index 0000000..71a3d2c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs @@ -0,0 +1,33 @@ +using UnityEngine; +using Unity.Collections; +using Pathfinding.Jobs; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Checks if nodes are obstructed by obstacles or not. + /// + /// See: + /// + struct JobCheckCollisions : IJobTimeSliced { + [ReadOnly] + public NativeArray nodePositions; + public NativeArray collisionResult; + public GraphCollision collision; + int startIndex; + + public void Execute () { + Execute(TimeSlice.Infinite); + } + + public bool Execute (TimeSlice timeSlice) { + for (int i = startIndex; i < nodePositions.Length; i++) { + collisionResult[i] = collisionResult[i] && collision.Check(nodePositions[i]); + if ((i & 127) == 0 && timeSlice.expired) { + startIndex = i + 1; + return false; + } + } + return true; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs.meta new file mode 100644 index 0000000..9311878 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCheckCollisions.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6977ffeeb891185449eae9f827667558 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs new file mode 100644 index 0000000..84ae59e --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs @@ -0,0 +1,28 @@ +#if UNITY_2022_2_OR_NEWER +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Fills the output with true or false depending on if the collider hit was a hit. + /// + /// result[i] = false if hits[i] is a valid hit, otherwise true. + /// + [BurstCompile] + public struct JobColliderHitsToBooleans : IJob { + [ReadOnly] + public NativeArray hits; + + [WriteOnly] + public NativeArray result; + + public void Execute () { + for (int i = 0; i < hits.Length; i++) { + result[i] = hits[i].instanceID == 0; + } + } + } +} +#endif diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs.meta new file mode 100644 index 0000000..39e8bb7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobColliderHitsToBooleans.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0e741e2fc3248564998e707ba25fe69c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs new file mode 100644 index 0000000..690448f --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs @@ -0,0 +1,50 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using Pathfinding.Jobs; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Copies 3D arrays with grid data from one shape to another. + /// + /// Only the data for the nodes that exist in both buffers will be copied. + /// + /// This essentially is several jobs in one (to avoid scheduling overhead). + /// See that job for more documentation. + /// + [BurstCompile] + public struct JobCopyBuffers : IJob { + [ReadOnly] + [DisableUninitializedReadCheck] + public GridGraphNodeData input; + + [WriteOnly] + public GridGraphNodeData output; + public IntBounds bounds; + + public bool copyPenaltyAndTags; + + public void Execute () { +#if ENABLE_UNITY_COLLECTIONS_CHECKS + if (!input.bounds.Contains(bounds)) throw new System.ArgumentException("Bounds are outside the source buffer"); + if (!output.bounds.Contains(bounds)) throw new System.ArgumentException("Bounds are outside the destination buffer"); +#endif + var inputSlice = new Slice3D(input.bounds, bounds); + var outputSlice = new Slice3D(output.bounds, bounds); + // Note: Having a single job that copies all of the buffers avoids a lot of scheduling overhead. + // We do miss out on parallelization, however for this job it is not that significant. + JobCopyRectangle.Copy(input.positions, output.positions, inputSlice, outputSlice); + JobCopyRectangle.Copy(input.normals, output.normals, inputSlice, outputSlice); + JobCopyRectangle.Copy(input.connections, output.connections, inputSlice, outputSlice); + if (copyPenaltyAndTags) { + JobCopyRectangle.Copy(input.penalties, output.penalties, inputSlice, outputSlice); + JobCopyRectangle.Copy(input.tags, output.tags, inputSlice, outputSlice); + } + JobCopyRectangle.Copy(input.walkable, output.walkable, inputSlice, outputSlice); + JobCopyRectangle.Copy(input.walkableWithErosion, output.walkableWithErosion, inputSlice, outputSlice); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs.meta new file mode 100644 index 0000000..9c60956 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobCopyBuffers.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a3c4dcf30e4497e44a9c7421803ec902 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs new file mode 100644 index 0000000..5b5ba35 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs @@ -0,0 +1,190 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using Pathfinding.Jobs; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Calculates erosion. + /// Note that to ensure that connections are completely up to date after updating a node you + /// have to calculate the connections for both the changed node and its neighbours. + /// + /// In a layered grid graph, this will recalculate the connections for all nodes + /// in the (x,z) cell (it may have multiple layers of nodes). + /// + /// See: CalculateConnections(GridNodeBase) + /// + [BurstCompile] + public struct JobErosion : IJob where AdjacencyMapper : GridAdjacencyMapper, new() { + public IntBounds bounds; + public IntBounds writeMask; + public NumNeighbours neighbours; + public int erosion; + public bool erosionUsesTags; + public int erosionStartTag; + + [ReadOnly] + public NativeArray nodeConnections; + + [ReadOnly] + public NativeArray nodeWalkable; + + [WriteOnly] + public NativeArray outNodeWalkable; + + public NativeArray nodeTags; + public int erosionTagsPrecedenceMask; + + // Note: the first 3 connections are to nodes with a higher x or z coordinate + // The last 3 connections are to nodes with a lower x or z coordinate + // This is required for the grassfire transform to work properly + // This is a permutation of GridGraph.hexagonNeighbourIndices + static readonly int[] hexagonNeighbourIndices = { 1, 2, 5, 0, 3, 7 }; + + public void Execute () { + var slice = new Slice3D(bounds, bounds); + var size = slice.slice.size; + slice.AssertMatchesOuter(nodeConnections); + slice.AssertMatchesOuter(nodeWalkable); + slice.AssertMatchesOuter(outNodeWalkable); + slice.AssertMatchesOuter(nodeTags); + Assert.IsTrue(bounds.Contains(writeMask)); + + var(outerStrideX, outerStrideY, outerStrideZ) = slice.outerStrides; + var(innerStrideX, innerStrideY, innerStrideZ) = slice.innerStrides; + NativeArray neighbourOffsets = new NativeArray(8, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * innerStrideZ + GridGraph.neighbourXOffsets[i] * innerStrideX; + + var erosionDistances = new NativeArray(slice.length, Allocator.Temp, NativeArrayOptions.ClearMemory); + var adjacencyMapper = new AdjacencyMapper(); + var layers = adjacencyMapper.LayerCount(slice.slice); + var outerOffset = slice.outerStartIndex; + if (neighbours == NumNeighbours.Six) { + // Use the grassfire transform: https://en.wikipedia.org/wiki/Grassfire_transform extended to hexagonal graphs + for (int z = 1; z < size.z - 1; z++) { + for (int x = 1; x < size.x - 1; x++) { + for (int y = 0; y < layers; y++) { + // Note: This is significantly faster than using csum, because burst can optimize it better + int outerIndex = z * outerStrideZ + x * outerStrideX + y * outerStrideY + outerOffset; + var innerIndexXZ = z * innerStrideZ + x * innerStrideX; + int innerIndex = innerIndexXZ + y * innerStrideY; + int v = int.MaxValue; + for (int i = 3; i < 6; i++) { + int connection = hexagonNeighbourIndices[i]; + if (!adjacencyMapper.HasConnection(outerIndex, connection, nodeConnections)) v = -1; + else v = math.min(v, erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, connection, nodeConnections, neighbourOffsets, innerStrideY)]); + } + + erosionDistances[innerIndex] = v + 1; + } + } + } + + for (int z = size.z - 2; z > 0; z--) { + for (int x = size.x - 2; x > 0; x--) { + for (int y = 0; y < layers; y++) { + int outerIndex = z * outerStrideZ + x * outerStrideX + y * outerStrideY + outerOffset; + var innerIndexXZ = z * innerStrideZ + x * innerStrideX; + int innerIndex = innerIndexXZ + y * innerStrideY; + int v = int.MaxValue; + for (int i = 3; i < 6; i++) { + int connection = hexagonNeighbourIndices[i]; + if (!adjacencyMapper.HasConnection(outerIndex, connection, nodeConnections)) v = -1; + else v = math.min(v, erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, connection, nodeConnections, neighbourOffsets, innerStrideY)]); + } + + erosionDistances[innerIndex] = math.min(erosionDistances[innerIndex], v + 1); + } + } + } + } else { + /* Index offset to get neighbour nodes. Added to a node's index to get a neighbour node index. + * + * \code + * Z + * | + * | + * + * 6 2 5 + * \ | / + * -- 3 - X - 1 ----- X + * / | \ + * 7 0 4 + * + * | + * | + * \endcode + */ + const int DirectionDown = 0; + const int DirectionRight = 1; + const int DirectionUp = 2; + const int DirectionLeft = 3; + + // Use the grassfire transform: https://en.wikipedia.org/wiki/Grassfire_transform + for (int z = 1; z < size.z - 1; z++) { + for (int x = 1; x < size.x - 1; x++) { + for (int y = 0; y < layers; y++) { + int outerIndex = z * outerStrideZ + x * outerStrideX + y * outerStrideY + outerOffset; + var innerIndexXZ = z * innerStrideZ + x * innerStrideX; + int innerIndex = innerIndexXZ + y * innerStrideY; + var v1 = -1; + if (adjacencyMapper.HasConnection(outerIndex, DirectionDown, nodeConnections)) v1 = erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, DirectionDown, nodeConnections, neighbourOffsets, innerStrideY)]; + var v2 = -1; + if (adjacencyMapper.HasConnection(outerIndex, DirectionLeft, nodeConnections)) v2 = erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, DirectionLeft, nodeConnections, neighbourOffsets, innerStrideY)]; + + erosionDistances[innerIndex] = math.min(v1, v2) + 1; + } + } + } + + for (int z = size.z - 2; z > 0; z--) { + for (int x = size.x - 2; x > 0; x--) { + for (int y = 0; y < layers; y++) { + int outerIndex = z * outerStrideZ + x * outerStrideX + y * outerStrideY + outerOffset; + var innerIndexXZ = z * innerStrideZ + x * innerStrideX; + int innerIndex = innerIndexXZ + y * innerStrideY; + var v1 = -1; + if (adjacencyMapper.HasConnection(outerIndex, DirectionUp, nodeConnections)) v1 = erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, DirectionUp, nodeConnections, neighbourOffsets, innerStrideY)]; + var v2 = -1; + if (adjacencyMapper.HasConnection(outerIndex, DirectionRight, nodeConnections)) v2 = erosionDistances[adjacencyMapper.GetNeighbourIndex(innerIndexXZ, innerIndex, DirectionRight, nodeConnections, neighbourOffsets, innerStrideY)]; + + erosionDistances[innerIndex] = math.min(erosionDistances[outerIndex], math.min(v1, v2) + 1); + } + } + } + } + + var relativeWriteMask = writeMask.Offset(-bounds.min); + + // Erosion tags are allowed to overwrite the ones the user specifies, as well as the ones that are already reserved for erosion. + for (int i = erosionStartTag; i < erosionStartTag + erosion; i++) erosionTagsPrecedenceMask |= 1 << i; + + for (int y = relativeWriteMask.min.y; y < relativeWriteMask.max.y; y++) { + for (int z = relativeWriteMask.min.z; z < relativeWriteMask.max.z; z++) { + for (int x = relativeWriteMask.min.x; x < relativeWriteMask.max.x; x++) { + int outerIndex = x * outerStrideX + y * outerStrideY + z * outerStrideZ + outerOffset; + int innerIndex = x * innerStrideX + y * innerStrideY + z * innerStrideZ; + if (erosionUsesTags) { + var prevTag = nodeTags[outerIndex]; + outNodeWalkable[outerIndex] = nodeWalkable[outerIndex]; + + if (erosionDistances[innerIndex] < erosion) { + if (((erosionTagsPrecedenceMask >> prevTag) & 0x1) != 0) { + nodeTags[outerIndex] = nodeWalkable[outerIndex] ? math.min(GraphNode.MaxTagIndex, erosionDistances[innerIndex] + erosionStartTag) : 0; + } + } else if (prevTag >= erosionStartTag && prevTag < erosionStartTag + erosion) { + // If the node already had a tag that was reserved for erosion, but it shouldn't have that tag, then we remove it. + nodeTags[outerIndex] = 0; + } + } else { + outNodeWalkable[outerIndex] = nodeWalkable[outerIndex] & (erosionDistances[innerIndex] >= erosion); + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs.meta new file mode 100644 index 0000000..09484b2 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobErosion.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 12ba2e44c6c183644a4ad6186f4ab21e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs new file mode 100644 index 0000000..6b40a5f --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs @@ -0,0 +1,128 @@ +using Unity.Burst; +using Unity.Collections; +using Unity.Mathematics; +using Pathfinding.Jobs; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Filters out diagonal connections that are not allowed in layered grid graphs. + /// + /// This is a IJobParallelForBatched job which is parallelelized over the z coordinate of the . + /// + /// The job will run first, and calculate the connections for all nodes. + /// However, for layered grid graphs, the connections for diagonal nodes may be incorrect, and this + /// post-processing pass is needed to validate the diagonal connections. + /// + [BurstCompile] + public struct JobFilterDiagonalConnections : IJobParallelForBatched { + public Slice3D slice; + public NumNeighbours neighbours; + public bool cutCorners; + + /// All bitpacked node connections + public UnsafeSpan nodeConnections; + + public bool allowBoundsChecks => false; + + public void Execute (int start, int count) { + slice.AssertMatchesOuter(nodeConnections); + + // For single layer graphs this will have already been done in the JobCalculateGridConnections job + // but for layered grid graphs we need to handle things differently because the data layout is different + + int3 size = slice.outerSize; + NativeArray neighbourOffsets = new NativeArray(8, Allocator.Temp, NativeArrayOptions.UninitializedMemory); + + for (int i = 0; i < 8; i++) neighbourOffsets[i] = GridGraph.neighbourZOffsets[i] * size.x + GridGraph.neighbourXOffsets[i]; + + ulong hexagonConnectionMask = 0; + for (int i = 0; i < GridGraph.hexagonNeighbourIndices.Length; i++) hexagonConnectionMask |= (ulong)LevelGridNode.ConnectionMask << (LevelGridNode.ConnectionStride*GridGraph.hexagonNeighbourIndices[i]); + + int adjacencyThreshold = cutCorners ? 1 : 2; + int layerStride = size.x * size.z; + start += slice.slice.min.z; + for (int y = slice.slice.min.y; y < slice.slice.max.y; y++) { + // The loop is parallelized over z coordinates + for (int z = start; z < start + count; z++) { + for (int x = slice.slice.min.x; x < slice.slice.max.x; x++) { + int nodeIndexXZ = z * size.x + x; + int nodeIndex = nodeIndexXZ + y * layerStride; + + switch (neighbours) { + case NumNeighbours.Four: + // Mask out all the diagonal connections + nodeConnections[nodeIndex] = nodeConnections[nodeIndex] | LevelGridNode.DiagonalConnectionsMask; + break; + case NumNeighbours.Eight: + var conns = nodeConnections[nodeIndex]; + + // Skip node if no connections are enabled already + if (conns == LevelGridNode.AllConnectionsMask) continue; + + // When cutCorners is enabled then the diagonal connection is allowed + // if at least one axis aligned connection is adjacent to this diagonal. + // Otherwise both axis aligned connections must be present. + // + // X ----- axis2 + // | \ + // | \ + // | \ + // axis1 diagonal + // + // Z + // | + // | + // + // 6 2 5 + // \ | / + // -- 3 - X - 1 ----- X + // / | \ + // 7 0 4 + // + // | + // | + // + for (int dir = 0; dir < 4; dir++) { + int adjacent = 0; + var axis1 = (conns >> dir*LevelGridNode.ConnectionStride) & LevelGridNode.ConnectionMask; + var axis2 = (conns >> ((dir+1) % 4)*LevelGridNode.ConnectionStride) & LevelGridNode.ConnectionMask; + var diagonal = (conns >> (dir+4)*LevelGridNode.ConnectionStride) & LevelGridNode.ConnectionMask; + + // Check if the diagonal connection is present at all. + // The JobCalculateGridConnections calculated this. + if (diagonal == LevelGridNode.NoConnection) continue; + + if (axis1 != LevelGridNode.NoConnection) { + // We also check that the neighbour node is also connected to the diagonal node + var neighbourDir = (dir + 1) % 4; + var neighbourIndex = nodeIndexXZ + neighbourOffsets[dir] + (int)axis1 * layerStride; + if (((nodeConnections[neighbourIndex] >> neighbourDir*LevelGridNode.ConnectionStride) & LevelGridNode.ConnectionMask) == diagonal) { + adjacent++; + } + } + if (axis2 != LevelGridNode.NoConnection) { + var neighbourDir = dir; + var neighbourIndex = nodeIndexXZ + neighbourOffsets[(dir+1)%4] + (int)axis2 * layerStride; + if (((nodeConnections[neighbourIndex] >> neighbourDir*LevelGridNode.ConnectionStride) & LevelGridNode.ConnectionMask) == diagonal) { + adjacent++; + } + } + + if (adjacent < adjacencyThreshold) conns |= (ulong)LevelGridNode.NoConnection << (dir + 4)*LevelGridNode.ConnectionStride; + } + nodeConnections[nodeIndex] = conns; + break; + case NumNeighbours.Six: + // Hexagon layout + // Note that for layered nodes NoConnection is all bits set (see LevelGridNode.NoConnection) + // So in contrast to the non-layered grid graph we do a bitwise OR here + nodeConnections[nodeIndex] = (nodeConnections[nodeIndex] | ~hexagonConnectionMask) & LevelGridNode.AllConnectionsMask; + break; + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs.meta new file mode 100644 index 0000000..a3d3d9b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobFilterDiagonalConnections.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e737565f0f6416f4ab1470d4f194ed25 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs new file mode 100644 index 0000000..61707dd --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs @@ -0,0 +1,31 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Calculates if either of the two input hits actually hit something. + /// + /// result[i] = true if hit1[i] or hit2[i] is a valid hit. + /// + /// A valid hit will always have a non-zero normal. + /// + [BurstCompile] + public struct JobMergeRaycastCollisionHits : IJob { + [ReadOnly] + public NativeArray hit1; + + [ReadOnly] + public NativeArray hit2; + + [WriteOnly] + public NativeArray result; + + public void Execute () { + for (int i = 0; i < hit1.Length; i++) { + result[i] = hit1[i].normal == Vector3.zero && hit2[i].normal == Vector3.zero; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs.meta new file mode 100644 index 0000000..73957ca --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobMergeRaycastCollisionHits.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: be7b697c92431a74a8db8ae716c3f0b6 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs new file mode 100644 index 0000000..e9bdaf3 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs @@ -0,0 +1,36 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; +using Pathfinding.Jobs; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Calculates the default node positions for a grid graph. + /// + /// The node positions will lie on the base plane of the grid graph. + /// + /// See: + /// + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobNodeGridLayout : IJob, GridIterationUtilities.ICellAction { + public Matrix4x4 graphToWorld; + public IntBounds bounds; + + [WriteOnly] + public NativeArray nodePositions; + + public static Vector3 NodePosition (Matrix4x4 graphToWorld, int x, int z) { + return graphToWorld.MultiplyPoint3x4(new Vector3(x + 0.5f, 0, z + 0.5f)); + } + + public void Execute () { + GridIterationUtilities.ForEachCellIn3DArray(bounds.size, ref this); + } + + public void Execute (uint innerIndex, int x, int y, int z) { + nodePositions[(int)innerIndex] = NodePosition(graphToWorld, x + bounds.min.x, z + bounds.min.z); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs.meta new file mode 100644 index 0000000..4a82431 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeGridLayout.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 7dba760f9d6d81e458c5122c0d28b2df +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs new file mode 100644 index 0000000..7629df7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs @@ -0,0 +1,73 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// Calculates for each grid node if it should be walkable or not + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobNodeWalkability : IJob { + /// + /// If true, use the normal of the raycast hit to check if the ground is flat enough to stand on. + /// + /// Any nodes with a steeper slope than will be made unwalkable. + /// + public bool useRaycastNormal; + /// Max slope in degrees + public float maxSlope; + /// Normalized up direction of the graph + public Vector3 up; + /// If true, nodes will be made unwalkable if no ground was found under them + public bool unwalkableWhenNoGround; + /// For layered grid graphs, if there's a node above another node closer than this distance, the lower node will be made unwalkable + public float characterHeight; + /// Number of nodes in each layer + public int layerStride; + + [ReadOnly] + public NativeArray nodePositions; + + public NativeArray nodeNormals; + + [WriteOnly] + public NativeArray nodeWalkable; + + public void Execute () { + // Cosinus of the max slope + float cosMaxSlopeAngle = math.cos(math.radians(maxSlope)); + float4 upNative = new float4(up.x, up.y, up.z, 0); + float3 upNative3 = upNative.xyz; + + for (int i = 0; i < nodeNormals.Length; i++) { + // walkable will be set to false if no ground was found (unless that setting has been disabled) + // The normal will only be non-zero if something was hit. + bool didHit = math.any(nodeNormals[i]); + var walkable = didHit; + if (!didHit && !unwalkableWhenNoGround && i < layerStride) { + walkable = true; + // If there was no hit, but we still want to make the node walkable, then we set the normal to the up direction + nodeNormals[i] = upNative; + } + + // Check if the node is on a slope steeper than permitted + if (walkable && useRaycastNormal && didHit) { + // Take the dot product to find out the cosine of the angle it has (faster than Vector3.Angle) + float angle = math.dot(nodeNormals[i], upNative); + + // Check if the ground is flat enough to stand on + if (angle < cosMaxSlopeAngle) { + walkable = false; + } + } + + // Check if there is a node above this one (layered grid graph only) + if (walkable && i + layerStride < nodeNormals.Length && math.any(nodeNormals[i + layerStride])) { + walkable = math.dot(upNative3, nodePositions[i + layerStride] - nodePositions[i]) >= characterHeight; + } + + nodeWalkable[i] = walkable; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs.meta new file mode 100644 index 0000000..2a3d418 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobNodeWalkability.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: a4ace8275be3d2f4b990df3672acb302 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs new file mode 100644 index 0000000..4644787 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs @@ -0,0 +1,45 @@ +#if UNITY_2022_2_OR_NEWER +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Collections.LowLevel.Unsafe; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Prepares a set of capsule commands for collision checking in a grid graph. + /// + /// See: + /// + [BurstCompile] + public struct JobPrepareCapsuleCommands : IJob { + public Vector3 direction; + public Vector3 originOffset; + public float radius; + public LayerMask mask; + public PhysicsScene physicsScene; + + [ReadOnly] + public NativeArray origins; + + [WriteOnly] + public NativeArray commands; + + public void Execute () { + var commandSpan = commands.AsUnsafeSpan(); + // It turns out it is faster to set all commands to the same value using MemCpyReplicate and then patch point0 and point1, + // rather than setting each command individually (about 30% faster even). + // MemCpy is very fast. + var queryParameters = new QueryParameters(mask, false, QueryTriggerInteraction.Ignore, false); + commandSpan.Fill(new OverlapCapsuleCommand(physicsScene, Vector3.zero, Vector3.zero, radius, queryParameters)); + + for (int i = 0; i < commandSpan.Length; i++) { + var origin = origins[i] + originOffset; + commandSpan[i].point0 = origin; + commandSpan[i].point1 = origin + direction; + } + } + } +} +#endif diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs.meta new file mode 100644 index 0000000..414cc20 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareCapsuleCommands.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: af594897c5b7f094585ba27f8be57d45 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs new file mode 100644 index 0000000..e05347d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs @@ -0,0 +1,61 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Collections.LowLevel.Unsafe; +using Pathfinding.Util; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Prepares a set of raycast commands for a grid graph. + /// + /// Each ray will start at from the node's position. The end point of the raycast will be the start point + . + /// + /// See: + /// + [BurstCompile] + public struct JobPrepareGridRaycast : IJob { + public Matrix4x4 graphToWorld; + public IntBounds bounds; + public Vector3 raycastOffset; + public Vector3 raycastDirection; + public LayerMask raycastMask; + public PhysicsScene physicsScene; + + [WriteOnly] + public NativeArray raycastCommands; + + public void Execute () { + float raycastLength = raycastDirection.magnitude; + var size = bounds.size; + + // In particular Unity 2022.2 seems to assert that RaycastCommands use normalized directions + var direction = raycastDirection.normalized; + var commands = raycastCommands.AsUnsafeSpan(); + + Assert.AreEqual(commands.Length, size.x * size.z); + +#if UNITY_2022_2_OR_NEWER + var queryParameters = new QueryParameters(raycastMask, false, QueryTriggerInteraction.Ignore, false); + // This is about 30% faster than setting each command individually. MemCpy is fast. + commands.Fill(new RaycastCommand(physicsScene, Vector3.zero, direction, queryParameters, raycastLength)); +#else + const int RaycastMaxHits = 1; +#endif + + for (int z = 0; z < size.z; z++) { + int zw = z * size.x; + for (int x = 0; x < size.x; x++) { + int idx = zw + x; + var pos = JobNodeGridLayout.NodePosition(graphToWorld, x + bounds.min.x, z + bounds.min.z); +#if UNITY_2022_2_OR_NEWER + commands[idx].from = pos + raycastOffset; +#else + commands[idx] = new RaycastCommand(pos + raycastOffset, direction, raycastLength, raycastMask, RaycastMaxHits); +#endif + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs.meta new file mode 100644 index 0000000..3800b46 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareGridRaycast.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9f081267224f64d44a99b9962d3d300a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs new file mode 100644 index 0000000..8a7fc7a --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs @@ -0,0 +1,51 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Collections.LowLevel.Unsafe; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Prepares a set of raycast commands for a grid graph. + /// + /// This is very similar to but it uses an array of origin points instead of a grid pattern. + /// + /// See: + /// + [BurstCompile] + public struct JobPrepareRaycasts : IJob { + public Vector3 direction; + public Vector3 originOffset; + public float distance; + public LayerMask mask; + public PhysicsScene physicsScene; + + [ReadOnly] + public NativeArray origins; + + [WriteOnly] + public NativeArray raycastCommands; + + public void Execute () { + // In particular Unity 2022.2 seems to assert that RaycastCommands use normalized directions + var direction = this.direction.normalized; + var commands = raycastCommands.AsUnsafeSpan(); + +#if UNITY_2022_2_OR_NEWER + var queryParameters = new QueryParameters(mask, false, QueryTriggerInteraction.Ignore, false); + var defaultCommand = new RaycastCommand(physicsScene, Vector3.zero, direction, queryParameters, distance); + // This is about 30% faster than setting each command individually. MemCpy is fast. + commands.Fill(defaultCommand); +#endif + + for (int i = 0; i < raycastCommands.Length; i++) { +#if UNITY_2022_2_OR_NEWER + commands[i].from = origins[i] + originOffset; +#else + raycastCommands[i] = new RaycastCommand(origins[i] + originOffset, direction, distance, mask, 1); +#endif + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs.meta new file mode 100644 index 0000000..c998e6b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareRaycasts.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 41ad6b54690bf4949a2974c3ca5d2503 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs new file mode 100644 index 0000000..50562a2 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs @@ -0,0 +1,42 @@ +#if UNITY_2022_2_OR_NEWER +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Collections.LowLevel.Unsafe; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Prepares a set of sphere commands for collision checking in a grid graph. + /// + /// See: + /// + [BurstCompile] + public struct JobPrepareSphereCommands : IJob { + public Vector3 originOffset; + public float radius; + public LayerMask mask; + public PhysicsScene physicsScene; + + [ReadOnly] + public NativeArray origins; + + [WriteOnly] + public NativeArray commands; + + public void Execute () { + var commandSpan = commands.AsUnsafeSpan(); + // It turns out it is faster to set all commands to the same value using MemCpyReplicate and then patch point, + // rather than setting each command individually. + var queryParameters = new QueryParameters(mask, false, QueryTriggerInteraction.Ignore, false); + commandSpan.Fill(new OverlapSphereCommand(physicsScene, Vector3.zero, radius, queryParameters)); + + for (int i = 0; i < commandSpan.Length; i++) { + var origin = origins[i] + originOffset; + commandSpan[i].point = origin; + } + } + } +} +#endif diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs.meta new file mode 100644 index 0000000..f5fbad1 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobPrepareSphereCommands.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: db154518b4b8b044d946bb6a9770bdbf +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs new file mode 100644 index 0000000..fd34a1d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs @@ -0,0 +1,121 @@ +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using UnityEngine; +using Pathfinding.Jobs; +using Unity.Mathematics; + +namespace Pathfinding.Jobs { + public struct JobRaycastAll { + int maxHits; + public readonly float minStep; + + NativeArray results; + NativeArray semiResults; + NativeArray commands; + public PhysicsScene physicsScene; + + [BurstCompile] + private struct JobCreateCommands : IJobParallelFor { + public NativeArray commands; + [ReadOnly] + public NativeArray raycastHits; + + public float minStep; + public PhysicsScene physicsScene; + + public void Execute (int index) { + var rayHit = raycastHits[index]; + + if (rayHit.normal != default(Vector3)) { + var previousCommand = commands[index]; + // Little hack to bypass same collider hit in specific cases + var point = rayHit.point + previousCommand.direction.normalized * minStep; + var distance = previousCommand.distance - (point - previousCommand.from).magnitude; +#if UNITY_2022_2_OR_NEWER + // TODO: In 2022.2 with the 'hit multiple faces' option, this whole class might be redundant. + var queryParameters = new QueryParameters(previousCommand.queryParameters.layerMask, false, QueryTriggerInteraction.Ignore, false); + commands[index] = new RaycastCommand(physicsScene, point, previousCommand.direction, queryParameters, distance); +#else + commands[index] = new RaycastCommand(point, previousCommand.direction, distance, previousCommand.layerMask, 1); +#endif + } else { +#if UNITY_2022_2_OR_NEWER + // Note: Using a default RaycastCommand may cause Unity to crash. + // This seems to be primarily because it assumes a non-zero direction. + commands[index] = new RaycastCommand(physicsScene, Vector3.zero, Vector3.up, new QueryParameters(0, false, QueryTriggerInteraction.Ignore, false), 1); +#else + commands[index] = new RaycastCommand(Vector3.zero, Vector3.up, 1, 0, 1); +#endif + } + } + } + + [BurstCompile] + private struct JobCombineResults : IJob { + public int maxHits; + [ReadOnly] + public NativeArray semiResults; + public NativeArray results; + + public void Execute () { + int layerStride = semiResults.Length / maxHits; + + for (int i = 0; i < layerStride; i++) { + int layerOffset = 0; + + for (int j = maxHits - 1; j >= 0; j--) { + if (math.any(semiResults[i + j*layerStride].normal)) { + results[i + layerOffset] = semiResults[i + j*layerStride]; + layerOffset += layerStride; + } + } + } + } + } + + /// Jobified version of Physics.RaycastNonAlloc. + /// Array of commands to perform. + /// Array to store results in. + /// PhysicsScene to use for the raycasts. Only used in Unity 2022.2 or later. + /// Max hits count per command. + /// Allocator to use for the results array. + /// Tracker to use for dependencies. + /// Minimal distance each Raycast should progress. + public JobRaycastAll(NativeArray commands, NativeArray results, PhysicsScene physicsScene, int maxHits, Allocator allocator, JobDependencyTracker dependencyTracker, float minStep = 0.0001f) { + if (maxHits <= 0) throw new System.ArgumentException("maxHits should be greater than zero"); + if (results.Length < commands.Length * maxHits) throw new System.ArgumentException("Results array length does not match maxHits count"); + if (minStep < 0f) throw new System.ArgumentException("minStep should be more or equal to zero"); + + this.results = results; + this.maxHits = maxHits; + this.minStep = minStep; + this.commands = commands; + this.physicsScene = physicsScene; + + semiResults = dependencyTracker.NewNativeArray(maxHits * commands.Length, allocator); + } + + public JobHandle Schedule (JobHandle dependency) { + for (int i = 0; i < maxHits; i++) { + var semiResultsPart = semiResults.GetSubArray(i*commands.Length, commands.Length); + dependency = RaycastCommand.ScheduleBatch(commands, semiResultsPart, 128, dependency); + if (i < maxHits - 1) { + var filter = new JobCreateCommands { + commands = commands, + raycastHits = semiResultsPart, + minStep = minStep, + physicsScene = physicsScene, + }; + dependency = filter.Schedule(commands.Length, 256, dependency); + } + } + + return new JobCombineResults { + semiResults = semiResults, + maxHits = maxHits, + results = results + }.Schedule(dependency); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs.meta new file mode 100644 index 0000000..aed1b42 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobRaycastAll.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9f8eb8ba655e2d2dc88958d073ff52f3 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs new file mode 100644 index 0000000..a94b652 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs @@ -0,0 +1,93 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Mathematics; +using Pathfinding.Jobs; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Reads node data from managed objects into unmanaged arrays. + /// + /// This is done so that burst jobs can later access this data directly. + /// + /// Later, data will be written back to the managed objects using the job. + /// + public struct JobReadNodeData : IJobParallelForBatched { + public System.Runtime.InteropServices.GCHandle nodesHandle; + public uint graphIndex; + + public Slice3D slice; + + [WriteOnly] + public NativeArray nodePositions; + + [WriteOnly] + public NativeArray nodePenalties; + + [WriteOnly] + public NativeArray nodeTags; + + [WriteOnly] + public NativeArray nodeConnections; + + [WriteOnly] + public NativeArray nodeWalkableWithErosion; + + [WriteOnly] + public NativeArray nodeWalkable; + + public bool allowBoundsChecks => false; + + struct Reader : GridIterationUtilities.ISliceAction { + public GridNodeBase[] nodes; + public NativeArray nodePositions; + public NativeArray nodePenalties; + public NativeArray nodeTags; + public NativeArray nodeConnections; + public NativeArray nodeWalkableWithErosion; + public NativeArray nodeWalkable; + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public void Execute (uint outerIdx, uint innerIdx) { + var dataIdx = (int)innerIdx; + // The data bounds may have more layers than the existing nodes if a new layer is being added. + // We can only copy from the nodes that exist. + if (outerIdx < nodes.Length) { + var node = nodes[outerIdx]; + if (node != null) { + nodePositions[dataIdx] = (Vector3)node.position; + nodePenalties[dataIdx] = node.Penalty; + nodeTags[dataIdx] = (int)node.Tag; + nodeConnections[dataIdx] = node is GridNode gn ? (ulong)gn.GetAllConnectionInternal() : (node as LevelGridNode).gridConnections; + nodeWalkableWithErosion[dataIdx] = node.Walkable; + nodeWalkable[dataIdx] = node.WalkableErosion; + return; + } + } + + // Fallback in case the node was null (only happens for layered grid graphs), + // or if we are adding more layers to the graph, in which case we are outside + // the bounds of the nodes array. + nodePositions[dataIdx] = Vector3.zero; + nodePenalties[dataIdx] = 0; + nodeTags[dataIdx] = 0; + nodeConnections[dataIdx] = 0; + nodeWalkableWithErosion[dataIdx] = false; + nodeWalkable[dataIdx] = false; + } + } + + public void Execute (int startIndex, int count) { + var reader = new Reader { + // This is a managed type, we need to trick Unity to allow this inside of a job + nodes = (GridNodeBase[])nodesHandle.Target, + nodePositions = nodePositions, + nodePenalties = nodePenalties, + nodeTags = nodeTags, + nodeConnections = nodeConnections, + nodeWalkableWithErosion = nodeWalkableWithErosion, + nodeWalkable = nodeWalkable + }; + GridIterationUtilities.ForEachCellIn3DSlice(slice, ref reader); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs.meta new file mode 100644 index 0000000..2092692 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobReadNodeData.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 203c976aeb6f3d84caeab084738a53c8 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs new file mode 100644 index 0000000..96a58a8 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs @@ -0,0 +1,91 @@ +using UnityEngine; +using Unity.Collections; +using Unity.Mathematics; +using Pathfinding.Jobs; +using UnityEngine.Assertions; + +namespace Pathfinding.Graphs.Grid.Jobs { + /// + /// Writes node data from unmanaged arrays into managed objects. + /// + /// This is done after burst jobs have been working on graph data, as they cannot access the managed objects directly. + /// + /// Earlier, data will have been either calculated from scratch, or read from the managed objects using the job. + /// + public struct JobWriteNodeData : IJobParallelForBatched { + public System.Runtime.InteropServices.GCHandle nodesHandle; + public uint graphIndex; + + /// (width, depth) of the array that the refers to + public int3 nodeArrayBounds; + public IntBounds dataBounds; + public IntBounds writeMask; + + [ReadOnly] + public NativeArray nodePositions; + + [ReadOnly] + public NativeArray nodePenalties; + + [ReadOnly] + public NativeArray nodeTags; + + [ReadOnly] + public NativeArray nodeConnections; + + [ReadOnly] + public NativeArray nodeWalkableWithErosion; + + [ReadOnly] + public NativeArray nodeWalkable; + + public bool allowBoundsChecks => false; + + public void Execute (int startIndex, int count) { + // This is a managed type, we need to trick Unity to allow this inside of a job + var nodes = (GridNodeBase[])nodesHandle.Target; + + var relativeMask = writeMask.Offset(-dataBounds.min); + + // Determinstically convert the indices to rows. It is much easier to process a number of whole rows. + var writeSize = writeMask.size; + var zstart = startIndex / (writeSize.x*writeSize.y); + var zend = (startIndex+count) / (writeSize.x*writeSize.y); + + Assert.IsTrue(zstart >= 0 && zstart <= writeSize.z); + Assert.IsTrue(zend >= 0 && zend <= writeSize.z); + relativeMask.min.z = writeMask.min.z + zstart - dataBounds.min.z; + relativeMask.max.z = writeMask.min.z + zend - dataBounds.min.z; + + var dataSize = dataBounds.size; + for (int y = relativeMask.min.y; y < relativeMask.max.y; y++) { + for (int z = relativeMask.min.z; z < relativeMask.max.z; z++) { + var rowOffset1 = (y*dataSize.z + z)*dataSize.x; + var rowOffset2 = (z + dataBounds.min.z)*nodeArrayBounds.x + dataBounds.min.x; + var rowOffset3 = (y + dataBounds.min.y)*nodeArrayBounds.z*nodeArrayBounds.x + rowOffset2; + for (int x = relativeMask.min.x; x < relativeMask.max.x; x++) { + int dataIndex = rowOffset1 + x; + int nodeIndex = rowOffset3 + x; + var node = nodes[nodeIndex]; + if (node != null) { + node.GraphIndex = graphIndex; + node.NodeInGridIndex = rowOffset2 + x; + // TODO: Use UnsafeSpan + node.position = (Int3)nodePositions[dataIndex]; + node.Penalty = nodePenalties[dataIndex]; + node.Tag = (uint)nodeTags[dataIndex]; + if (node is GridNode gridNode) { + gridNode.SetAllConnectionInternal((int)nodeConnections[dataIndex]); + } else if (node is LevelGridNode levelGridNode) { + levelGridNode.LayerCoordinateInGrid = y + dataBounds.min.y; + levelGridNode.SetAllConnectionInternal(nodeConnections[dataIndex]); + } + node.Walkable = nodeWalkableWithErosion[dataIndex]; + node.WalkableErosion = nodeWalkable[dataIndex]; + } + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs.meta new file mode 100644 index 0000000..503743b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Jobs/JobWriteNodeData.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 86fcb7ce1d754024290197052acf39d5 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules.meta new file mode 100644 index 0000000..ab73ead --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: e96df07c8616c534c99a64575f066a3d +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs new file mode 100644 index 0000000..174b7d2 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs @@ -0,0 +1,293 @@ +using System.Collections.Generic; + +namespace Pathfinding.Graphs.Grid.Rules { + using Pathfinding.Serialization; + using Pathfinding.Jobs; + using Unity.Jobs; + using Unity.Collections; + using Unity.Mathematics; + + public class CustomGridGraphRuleEditorAttribute : System.Attribute { + public System.Type type; + public string name; + public CustomGridGraphRuleEditorAttribute(System.Type type, string name) { + this.type = type; + this.name = name; + } + } + + /// + /// Container for all rules in a grid graph. + /// + /// + /// // Get the first grid graph in the scene + /// var gridGraph = AstarPath.active.data.gridGraph; + /// + /// gridGraph.rules.AddRule(new Pathfinding.Graphs.Grid.Rules.RuleAnglePenalty { + /// penaltyScale = 10000, + /// curve = AnimationCurve.Linear(0, 0, 90, 1), + /// }); + /// + /// + /// See: + /// See: grid-rules (view in online documentation for working links) + /// + [JsonOptIn] + public class GridGraphRules { + List >[] jobSystemCallbacks; + List >[] mainThreadCallbacks; + + /// List of all rules + [JsonMember] + List rules = new List(); + + long lastHash; + + /// Context for when scanning or updating a graph + public class Context { + /// Graph which is being scanned or updated + public GridGraph graph; + /// Data for all the nodes as NativeArrays + public GridGraphScanData data; + /// + /// Tracks dependencies between jobs to allow parallelism without tediously specifying dependencies manually. + /// Always use when scheduling jobs. + /// + public JobDependencyTracker tracker => data.dependencyTracker; + } + + public void AddRule (GridGraphRule rule) { + rules.Add(rule); + lastHash = -1; + } + + public void RemoveRule (GridGraphRule rule) { + rules.Remove(rule); + lastHash = -1; + } + + public IReadOnlyList GetRules () { + if (rules == null) rules = new List(); + return rules.AsReadOnly(); + } + + long Hash () { + long hash = 196613; + + for (int i = 0; i < rules.Count; i++) { + if (rules[i] != null && rules[i].enabled) hash = hash * 1572869 ^ (long)rules[i].Hash; + } + return hash; + } + + public void RebuildIfNecessary () { + var newHash = Hash(); + + if (newHash == lastHash && jobSystemCallbacks != null && mainThreadCallbacks != null) return; + lastHash = newHash; + Rebuild(); + } + + public void Rebuild () { + rules = rules ?? new List(); + jobSystemCallbacks = jobSystemCallbacks ?? new List >[6]; + for (int i = 0; i < jobSystemCallbacks.Length; i++) { + if (jobSystemCallbacks[i] != null) jobSystemCallbacks[i].Clear(); + } + mainThreadCallbacks = mainThreadCallbacks ?? new List >[6]; + for (int i = 0; i < mainThreadCallbacks.Length; i++) { + if (mainThreadCallbacks[i] != null) mainThreadCallbacks[i].Clear(); + } + for (int i = 0; i < rules.Count; i++) { + if (rules[i].enabled) rules[i].Register(this); + } + } + + public void DisposeUnmanagedData () { + if (rules != null) { + for (int i = 0; i < rules.Count; i++) { + if (rules[i] != null) { + rules[i].DisposeUnmanagedData(); + rules[i].SetDirty(); + } + } + } + } + + static void CallActions (List > actions, Context context) { + if (actions != null) { + try { + for (int i = 0; i < actions.Count; i++) actions[i](context); + } catch (System.Exception e) { + UnityEngine.Debug.LogException(e); + } + } + } + + /// + /// Executes the rules for the given pass. + /// Call handle.Complete on, or wait for, all yielded job handles. + /// + public IEnumerator ExecuteRule (GridGraphRule.Pass rule, Context context) { + if (jobSystemCallbacks == null) Rebuild(); + CallActions(jobSystemCallbacks[(int)rule], context); + + if (mainThreadCallbacks[(int)rule] != null && mainThreadCallbacks[(int)rule].Count > 0) { + if (!context.tracker.forceLinearDependencies) yield return context.tracker.AllWritesDependency; + CallActions(mainThreadCallbacks[(int)rule], context); + } + } + + public void ExecuteRuleMainThread (GridGraphRule.Pass rule, Context context) { + if (jobSystemCallbacks[(int)rule] != null && jobSystemCallbacks[(int)rule].Count > 0) throw new System.Exception("A job system pass has been added for the " + rule + " pass. " + rule + " only supports main thread callbacks."); + if (context.tracker != null) context.tracker.AllWritesDependency.Complete(); + CallActions(mainThreadCallbacks[(int)rule], context); + } + + /// + /// Adds a pass callback that uses the job system. + /// This rule should only schedule jobs using the `Context.tracker` dependency tracker. Data is not safe to access directly in the callback + /// + /// This method should only be called from rules in their Register method. + /// + public void AddJobSystemPass (GridGraphRule.Pass pass, System.Action action) { + var index = (int)pass; + + if (jobSystemCallbacks[index] == null) { + jobSystemCallbacks[index] = new List >(); + } + jobSystemCallbacks[index].Add(action); + } + + /// + /// Adds a pass callback that runs in the main thread. + /// The callback may access and modify any data in the context. + /// You do not need to schedule jobs in order to access the data. + /// + /// Warning: Not all data in the Context is valid for every pass. For example you cannot access node connections in the BeforeConnections pass + /// since they haven't been calculated yet. + /// + /// This is a bit slower than since parallelism and the burst compiler cannot be used. + /// But if you need to use non-thread-safe APIs or data then this is a good choice. + /// + /// This method should only be called from rules in their Register method. + /// + public void AddMainThreadPass (GridGraphRule.Pass pass, System.Action action) { + var index = (int)pass; + + if (mainThreadCallbacks[index] == null) { + mainThreadCallbacks[index] = new List >(); + } + mainThreadCallbacks[index].Add(action); + } + + /// Deprecated: Use AddJobSystemPass or AddMainThreadPass instead + [System.Obsolete("Use AddJobSystemPass or AddMainThreadPass instead")] + public void Add (GridGraphRule.Pass pass, System.Action action) { + AddJobSystemPass(pass, action); + } + } + + /// + /// Custom rule for a grid graph. + /// See: + /// See: grid-rules (view in online documentation for working links) + /// + [JsonDynamicType] + // Compatibility with old versions + [JsonDynamicTypeAlias("Pathfinding.RuleTexture", typeof(RuleTexture))] + [JsonDynamicTypeAlias("Pathfinding.RuleAnglePenalty", typeof(RuleAnglePenalty))] + [JsonDynamicTypeAlias("Pathfinding.RuleElevationPenalty", typeof(RuleElevationPenalty))] + [JsonDynamicTypeAlias("Pathfinding.RulePerLayerModifications", typeof(RulePerLayerModifications))] + public abstract class GridGraphRule { + /// Only enabled rules are executed + [JsonMember] + public bool enabled = true; + int dirty = 1; + + /// + /// Where in the scanning process a rule will be executed. + /// Check the documentation for to see which data fields are valid in which passes. + /// + public enum Pass { + /// + /// Before the collision testing phase but after height testing. + /// This is very early. Most data is not valid by this point. + /// + /// You can use this if you want to modify the node positions and still have it picked up by the collision testing code. + /// + BeforeCollision, + /// + /// Before connections are calculated. + /// At this point height testing and collision testing has been done (if they are enabled). + /// + /// This is the most common pass to use. + /// If you are modifying walkability here then connections and erosion will be calculated properly. + /// + BeforeConnections, + /// + /// After connections are calculated. + /// + /// If you are modifying connections directly you should do that in this pass. + /// + /// Note: If erosion is used then this pass will be executed twice. One time before erosion and one time after erosion + /// when the connections are calculated again. + /// + AfterConnections, + /// + /// After erosion is calculated but before connections have been recalculated. + /// + /// If no erosion is used then this pass will not be executed. + /// + AfterErosion, + /// + /// After everything else. + /// This pass is executed after everything else is done. + /// You should not modify walkability in this pass because then the node connections will not be up to date. + /// + PostProcess, + /// + /// After the graph update has been applied to the graph. + /// + /// This pass can only be added as a main-thread pass. + /// + /// Warning: No native data in the context is valid at this point. It has all been disposed. + /// You cannot modify any data in this pass. + /// + AfterApplied, + } + + /// + /// Hash of the settings for this rule. + /// The method will be called again whenever the hash changes. + /// If the hash does not change it is assumed that the method does not need to be called again. + /// + public virtual int Hash => dirty; + + /// + /// Call if you have changed any setting of the rule. + /// This will ensure that any cached data the rule uses is rebuilt. + /// If you do not do this then any settings changes may not affect the graph when it is rescanned or updated. + /// + /// The purpose of this method call is to cause the property to change. If your custom rule overrides the Hash property to + /// return a hash of some settings, then you do not need to call this method for the changes the hash function already accounts for. + /// + public virtual void SetDirty () { + dirty++; + } + + /// + /// Called when the rule is removed or the graph is destroyed. + /// Use this to e.g. clean up any NativeArrays that the rule uses. + /// + /// Note: The rule should remain valid after this method has been called. + /// However the method is guaranteed to be called before the rule is executed again. + /// + public virtual void DisposeUnmanagedData () { + } + + /// Does preprocessing and adds callbacks to the object + public virtual void Register (GridGraphRules rules) { + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs.meta new file mode 100644 index 0000000..a73e50c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/GridGraphRules.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4d90c9a7bca49464796933f43b5506fc +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs new file mode 100644 index 0000000..1322770 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs @@ -0,0 +1,81 @@ +namespace Pathfinding.Graphs.Grid.Rules { + using Pathfinding.Jobs; + using Unity.Jobs; + using Unity.Collections; + using Unity.Burst; + using UnityEngine; + using Unity.Mathematics; + + /// + /// Applies penalty based on the slope of the surface below the node. + /// + /// This is useful if you for example want to discourage agents from walking on steep slopes. + /// + /// The penalty applied is equivalent to: + /// + /// + /// penalty = curve.evaluate(slope angle in degrees) * penaltyScale + /// + /// + /// [Open online documentation to see images] + /// + /// See: grid-rules (view in online documentation for working links) + /// + [Pathfinding.Util.Preserve] + public class RuleAnglePenalty : GridGraphRule { + public float penaltyScale = 10000; + public AnimationCurve curve = AnimationCurve.Linear(0, 0, 90, 1); + NativeArray angleToPenalty; + + public override void Register (GridGraphRules rules) { + if (!angleToPenalty.IsCreated) angleToPenalty = new NativeArray(32, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < angleToPenalty.Length; i++) { + angleToPenalty[i] = Mathf.Max(0, curve.Evaluate(90.0f * i / (angleToPenalty.Length - 1)) * penaltyScale); + } + + rules.AddJobSystemPass(Pass.BeforeConnections, context => { + new JobPenaltyAngle { + angleToPenalty = angleToPenalty, + up = context.data.up, + nodeNormals = context.data.nodes.normals, + penalty = context.data.nodes.penalties, + }.Schedule(context.tracker); + }); + } + + public override void DisposeUnmanagedData () { + if (angleToPenalty.IsCreated) angleToPenalty.Dispose(); + } + + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobPenaltyAngle : IJob { + public Vector3 up; + + [ReadOnly] + public NativeArray angleToPenalty; + + [ReadOnly] + public NativeArray nodeNormals; + + public NativeArray penalty; + + public void Execute () { + float4 up = new float4(this.up.x, this.up.y, this.up.z, 0); + + for (int i = 0; i < penalty.Length; i++) { + float4 normal = nodeNormals[i]; + if (math.any(normal)) { + float angle = math.acos(math.dot(normal, up)); + // Take the dot product to find out the cosinus of the angle it has + // Add penalty based on the angle from a precalculated array + float x = angle*(angleToPenalty.Length - 1)/math.PI; + int ix = (int)x; + float p1 = angleToPenalty[math.max(ix, 0)]; + float p2 = angleToPenalty[math.min(ix + 1, angleToPenalty.Length - 1)]; + penalty[i] += (uint)math.lerp(p1, p2, x - ix); + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs.meta new file mode 100644 index 0000000..d047088 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleAnglePenalty.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 40d5c4aeb2276457f8fe040e4c5d71fe +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs new file mode 100644 index 0000000..6f62660 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs @@ -0,0 +1,76 @@ +namespace Pathfinding.Graphs.Grid.Rules { + using Pathfinding.Jobs; + using Unity.Jobs; + using Unity.Collections; + using Unity.Burst; + using UnityEngine; + using Unity.Mathematics; + + /// + /// Applies penalty based on the elevation of the node. + /// + /// This is useful if you for example want to discourage agents from walking high up in mountain regions. + /// + /// The penalty applied is equivalent to: + /// + /// + /// penalty = curve.evaluate(Mathf.Clamp01(Mathf.InverseLerp(lower elevation range, upper elevation range, elevation))) * penaltyScale + /// + /// + /// [Open online documentation to see images] + /// + /// See: grid-rules (view in online documentation for working links) + /// + [Pathfinding.Util.Preserve] + public class RuleElevationPenalty : GridGraphRule { + public float penaltyScale = 10000; + public Vector2 elevationRange = new Vector2(0, 100); + public AnimationCurve curve = AnimationCurve.Linear(0, 0, 1, 1); + NativeArray elevationToPenalty; + + public override void Register (GridGraphRules rules) { + if (!elevationToPenalty.IsCreated) elevationToPenalty = new NativeArray(64, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + for (int i = 0; i < elevationToPenalty.Length; i++) { + elevationToPenalty[i] = Mathf.Max(0, penaltyScale * curve.Evaluate(i * 1.0f / (elevationToPenalty.Length - 1))); + } + + var clampedElevationRange = new Vector2(math.max(0, elevationRange.x), math.max(1, elevationRange.y)); + rules.AddJobSystemPass(Pass.BeforeConnections, context => { + //var elevationRangeScale = Matrix4x4.TRS(new Vector3(0, -clampedElevationRange.x, 0), Quaternion.identity, new Vector3(1, 1/(clampedElevationRange.y - clampedElevationRange.x), 1)); + var elevationRangeScale = Matrix4x4.Scale(new Vector3(1, 1/(clampedElevationRange.y - clampedElevationRange.x), 1)) * Matrix4x4.Translate(new Vector3(0, -clampedElevationRange.x, 0)); + new JobElevationPenalty { + elevationToPenalty = elevationToPenalty, + nodePositions = context.data.nodes.positions, + worldToGraph = elevationRangeScale * context.data.transform.matrix.inverse, + penalty = context.data.nodes.penalties, + }.Schedule(context.tracker); + }); + } + + public override void DisposeUnmanagedData () { + if (elevationToPenalty.IsCreated) elevationToPenalty.Dispose(); + } + + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobElevationPenalty : IJob { + [ReadOnly] + public NativeArray elevationToPenalty; + + [ReadOnly] + public NativeArray nodePositions; + + public Matrix4x4 worldToGraph; + public NativeArray penalty; + + public void Execute () { + for (int i = 0; i < penalty.Length; i++) { + float y = math.clamp(worldToGraph.MultiplyPoint3x4(nodePositions[i]).y, 0, 1) * (elevationToPenalty.Length - 1); + int iy = (int)y; + float p1 = elevationToPenalty[iy]; + float p2 = elevationToPenalty[math.min(iy + 1, elevationToPenalty.Length - 1)]; + penalty[i] += (uint)math.lerp(p1, p2, y - iy); + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs.meta new file mode 100644 index 0000000..5878475 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleElevationPenalty.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d2933158d922e49e39a332d795d26d68 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs new file mode 100644 index 0000000..1684b04 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs @@ -0,0 +1,79 @@ +using Pathfinding.Jobs; + +namespace Pathfinding.Graphs.Grid.Rules { + /// + /// Modifies nodes based on the layer of the surface under the node. + /// + /// You can for example make all surfaces with a specific layer make the nodes get a specific tag. + /// + /// [Open online documentation to see images] + /// + /// See: grid-rules (view in online documentation for working links) + /// + [Pathfinding.Util.Preserve] + public class RulePerLayerModifications : GridGraphRule { + public PerLayerRule[] layerRules = new PerLayerRule[0]; + const int SetTagBit = 1 << 30; + + public struct PerLayerRule { + /// Layer this rule applies to + public int layer; + /// The action to apply to matching nodes + public RuleAction action; + /// + /// Tag for the RuleAction.SetTag action. + /// Must be between 0 and + /// + public int tag; + } + + public enum RuleAction { + /// Sets the tag of all affected nodes to + SetTag, + /// Makes all affected nodes unwalkable + MakeUnwalkable, + } + + public override void Register (GridGraphRules rules) { + int[] layerToTag = new int[32]; + bool[] layerToUnwalkable = new bool[32]; + for (int i = 0; i < layerRules.Length; i++) { + var rule = layerRules[i]; + if (rule.action == RuleAction.SetTag) { + layerToTag[rule.layer] = SetTagBit | rule.tag; + } else { + layerToUnwalkable[rule.layer] = true; + } + } + + rules.AddMainThreadPass(Pass.BeforeConnections, context => { + if (!context.data.heightHits.IsCreated) { + UnityEngine.Debug.LogError("RulePerLayerModifications requires height testing to be enabled on the grid graph", context.graph.active); + return; + } + + var raycastHits = context.data.heightHits; + var nodeWalkable = context.data.nodes.walkable; + var nodeTags = context.data.nodes.tags; + var slice = new Slice3D(context.data.nodes.bounds, context.data.heightHitsBounds); + var size = slice.slice.size; + for (int y = 0; y < size.y; y++) { + for (int z = 0; z < size.z; z++) { + var rowOffset = y * size.x * size.z + z * size.x; + for (int x = 0; x < size.x; x++) { + var innerIndex = rowOffset + x; + var outerIndex = slice.InnerCoordinateToOuterIndex(x, y, z); + var coll = raycastHits[innerIndex].collider; + if (coll != null) { + var layer = coll.gameObject.layer; + if (layerToUnwalkable[layer]) nodeWalkable[outerIndex] = false; + var tag = layerToTag[layer]; + if ((tag & SetTagBit) != 0) nodeTags[outerIndex] = tag & 0xFF; + } + } + } + } + }); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs.meta new file mode 100644 index 0000000..bfa3859 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RulePerLayerModifications.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4869e55551c0e4e1abaaf19bcc3d44a1 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs new file mode 100644 index 0000000..ddc6a9c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs @@ -0,0 +1,181 @@ +using UnityEngine; +using Unity.Burst; +using Unity.Collections; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Pathfinding.Graphs.Grid.Rules { + using Pathfinding.Jobs; + + /// + /// Modifies nodes based on the contents of a texture. + /// + /// This can be used to "paint" penalties or walkability using an external program such as Photoshop. + /// + /// [Open online documentation to see images] + /// + /// See: grid-rules (view in online documentation for working links) + /// + [Pathfinding.Util.Preserve] + public class RuleTexture : GridGraphRule { + public Texture2D texture; + + public ChannelUse[] channels = new ChannelUse[4]; + public float[] channelScales = { 1000, 1000, 1000, 1000 }; + + public ScalingMode scalingMode = ScalingMode.StretchToFitGraph; + public float nodesPerPixel = 1; + + NativeArray colors; + + public enum ScalingMode { + FixedScale, + StretchToFitGraph, + } + + public override int Hash { + get { + var h = base.Hash ^ (texture != null ? (int)texture.updateCount : 0); +#if UNITY_EDITOR + if (texture != null) h ^= (int)texture.imageContentsHash.GetHashCode(); +#endif + return h; + } + } + + public enum ChannelUse { + None, + /// Penalty goes from 0 to channelScale depending on the channel value + Penalty, + /// Node Y coordinate goes from 0 to channelScale depending on the channel value + Position, + /// If channel value is zero the node is made unwalkable, penalty goes from 0 to channelScale depending on the channel value + WalkablePenalty, + /// If channel value is zero the node is made unwalkable + Walkable, + } + + public override void Register (GridGraphRules rules) { + if (texture == null) return; + + if (!texture.isReadable) { + Debug.LogError("Texture for the texture rule on a grid graph is not marked as readable.", texture); + return; + } + + if (colors.IsCreated) colors.Dispose(); + colors = new NativeArray(texture.GetPixels32(), Allocator.Persistent).Reinterpret(); + + // Make sure this is done outside the delegate, just in case the texture is later resized + var textureSize = new int2(texture.width, texture.height); + + float4 channelPenaltiesCombined = float4.zero; + bool4 channelDeterminesWalkability = false; + float4 channelPositionScalesCombined = float4.zero; + for (int i = 0; i < 4; i++) { + channelPenaltiesCombined[i] = channels[i] == ChannelUse.Penalty || channels[i] == ChannelUse.WalkablePenalty ? channelScales[i] : 0; + channelDeterminesWalkability[i] = channels[i] == ChannelUse.Walkable || channels[i] == ChannelUse.WalkablePenalty; + channelPositionScalesCombined[i] = channels[i] == ChannelUse.Position ? channelScales[i] : 0; + } + + channelPositionScalesCombined /= 255.0f; + channelPenaltiesCombined /= 255.0f; + + if (math.any(channelPositionScalesCombined)) { + rules.AddJobSystemPass(Pass.BeforeCollision, context => { + new JobTexturePosition { + colorData = colors, + nodePositions = context.data.nodes.positions, + nodeNormals = context.data.nodes.normals, + bounds = context.data.nodes.bounds, + colorDataSize = textureSize, + scale = scalingMode == ScalingMode.FixedScale ? 1.0f/math.max(0.01f, nodesPerPixel) : textureSize / new float2(context.graph.width, context.graph.depth), + channelPositionScale = channelPositionScalesCombined, + graphToWorld = context.data.transform.matrix, + }.Schedule(context.tracker); + }); + } + + rules.AddJobSystemPass(Pass.BeforeConnections, context => { + new JobTexturePenalty { + colorData = colors, + penalty = context.data.nodes.penalties, + walkable = context.data.nodes.walkable, + nodeNormals = context.data.nodes.normals, + bounds = context.data.nodes.bounds, + colorDataSize = textureSize, + scale = scalingMode == ScalingMode.FixedScale ? 1.0f/math.max(0.01f, nodesPerPixel) : textureSize / new float2(context.graph.width, context.graph.depth), + channelPenalties = channelPenaltiesCombined, + channelDeterminesWalkability = channelDeterminesWalkability, + }.Schedule(context.tracker); + }); + } + + public override void DisposeUnmanagedData () { + if (colors.IsCreated) colors.Dispose(); + } + + [BurstCompile] + public struct JobTexturePosition : IJob, GridIterationUtilities.INodeModifier { + [ReadOnly] + public NativeArray colorData; + [WriteOnly] + public NativeArray nodePositions; + [ReadOnly] + public NativeArray nodeNormals; + + public Matrix4x4 graphToWorld; + public IntBounds bounds; + public int2 colorDataSize; + public float2 scale; + public float4 channelPositionScale; + + public void ModifyNode (int dataIndex, int dataX, int dataLayer, int dataZ) { + var offset = bounds.min.xz; + int2 colorPos = math.clamp((int2)math.round((new float2(dataX, dataZ) + offset) * scale), int2.zero, colorDataSize - new int2(1, 1)); + int colorIndex = colorPos.y*colorDataSize.x + colorPos.x; + + int4 color = new int4((colorData[colorIndex] >> 0) & 0xFF, (colorData[colorIndex] >> 8) & 0xFF, (colorData[colorIndex] >> 16) & 0xFF, (colorData[colorIndex] >> 24) & 0xFF); + + float y = math.dot(channelPositionScale, color); + + nodePositions[dataIndex] = graphToWorld.MultiplyPoint3x4(new Vector3((bounds.min.x + dataX) + 0.5f, y, (bounds.min.z + dataZ) + 0.5f)); + } + + public void Execute () { + GridIterationUtilities.ForEachNode(bounds.size, nodeNormals, ref this); + } + } + + [BurstCompile] + public struct JobTexturePenalty : IJob, GridIterationUtilities.INodeModifier { + [ReadOnly] + public NativeArray colorData; + public NativeArray penalty; + public NativeArray walkable; + [ReadOnly] + public NativeArray nodeNormals; + + public IntBounds bounds; + public int2 colorDataSize; + public float2 scale; + public float4 channelPenalties; + public bool4 channelDeterminesWalkability; + + public void ModifyNode (int dataIndex, int dataX, int dataLayer, int dataZ) { + var offset = bounds.min.xz; + int2 colorPos = math.clamp((int2)math.round((new float2(dataX, dataZ) + offset) * scale), int2.zero, colorDataSize - new int2(1, 1)); + int colorIndex = colorPos.y*colorDataSize.x + colorPos.x; + + int4 color = new int4((colorData[colorIndex] >> 0) & 0xFF, (colorData[colorIndex] >> 8) & 0xFF, (colorData[colorIndex] >> 16) & 0xFF, (colorData[colorIndex] >> 24) & 0xFF); + + penalty[dataIndex] += (uint)math.dot(channelPenalties, color); + walkable[dataIndex] = walkable[dataIndex] & !math.any(channelDeterminesWalkability & (color == 0)); + } + + public void Execute () { + GridIterationUtilities.ForEachNode(bounds.size, nodeNormals, ref this); + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs.meta new file mode 100644 index 0000000..6b3c4aa --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Grid/Rules/RuleTexture.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 42c128143490d447fa6420a4f35fe9bb +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/GridGraph.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/GridGraph.cs new file mode 100644 index 0000000..ccf5675 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/GridGraph.cs @@ -0,0 +1,3681 @@ +using System.Collections.Generic; +using Math = System.Math; +using UnityEngine; +using System.Linq; +using UnityEngine.Profiling; + + +namespace Pathfinding { + using Pathfinding.Serialization; + using Pathfinding.Util; + using Unity.Collections; + using Unity.Jobs; + using Unity.Mathematics; + using Pathfinding.Jobs; + using Pathfinding.Graphs.Grid.Jobs; + using Unity.Burst; + using Pathfinding.Drawing; + using Pathfinding.Graphs.Grid; + using Pathfinding.Graphs.Grid.Rules; + using UnityEngine.Assertions; + + /// + /// Generates a grid of nodes. + /// [Open online documentation to see images] + /// The GridGraph does exactly what the name implies, generates nodes in a grid pattern. + /// + /// Grid graphs are excellent for when you already have a grid-based world. But they also work well for free-form worlds. + /// + /// Features: + /// - Throw any scene at it, and with minimal configurations you can get a good graph from it. + /// - Predictable pattern. + /// - Grid graphs work well with penalties and tags. + /// - You can update parts of the graph during runtime. + /// - Graph updates are fast. + /// - Scanning the graph is comparatively fast. + /// - Supports linecasting. + /// - Supports the funnel modifier. + /// - Supports both 2D and 3D physics. + /// - Supports isometric and hexagonal node layouts. + /// - Can apply penalty and walkability values from a supplied image. + /// - Perfect for terrains since it can make nodes walkable or unwalkable depending on the slope. + /// - Only supports a single layer, but you can use a if you need more layers. + /// + /// [Open online documentation to see images] + /// + /// Inspector + /// + /// \inspectorField{Shape, inspectorGridMode} + /// \inspectorField{2D, is2D} + /// \inspectorField{Align to tilemap, AlignToTilemap} + /// \inspectorField{Width, width} + /// \inspectorField{Depth, depth} + /// \inspectorField{Node size, nodeSize} + /// \inspectorField{Aspect ratio (isometric/advanced shape), aspectRatio} + /// \inspectorField{Isometric angle (isometric/advanced shape), isometricAngle} + /// \inspectorField{Center, center} + /// \inspectorField{Rotation, rotation} + /// \inspectorField{Connections, neighbours} + /// \inspectorField{Cut corners, cutCorners} + /// \inspectorField{Max step height, maxStepHeight} + /// \inspectorField{Account for slopes, maxStepUsesSlope} + /// \inspectorField{Max slope, maxSlope} + /// \inspectorField{Erosion iterations, erodeIterations} + /// \inspectorField{Use 2D physics, collision.use2D} + /// + /// Collision testing + /// \inspectorField{Collider type, collision.type} + /// \inspectorField{Diameter, collision.diameter} + /// \inspectorField{Height/length, collision.height} + /// \inspectorField{Offset, collision.collisionOffset} + /// \inspectorField{Obstacle layer mask, collision.mask} + /// \inspectorField{Preview, GridGraphEditor.collisionPreviewOpen} + /// + /// Height testing + /// \inspectorField{Ray length, collision.fromHeight} + /// \inspectorField{Mask, collision.heightMask} + /// \inspectorField{Thick raycast, collision.thickRaycast} + /// \inspectorField{Unwalkable when no ground, collision.unwalkableWhenNoGround} + /// + /// Rules + /// Take a look at grid-rules (view in online documentation for working links) for a list of available rules. + /// + /// Other settings + /// \inspectorField{Show surface, showMeshSurface} + /// \inspectorField{Show outline, showMeshOutline} + /// \inspectorField{Show connections, showNodeConnections} + /// \inspectorField{Initial penalty, NavGraph.initialPenalty} + /// + /// Updating the graph during runtime + /// Any graph which implements the IUpdatableGraph interface can be updated during runtime. + /// For grid graphs this is a great feature since you can update only a small part of the grid without causing any lag like a complete rescan would. + /// + /// If you for example just have instantiated an obstacle in the scene and you want to update the grid where that obstacle was instantiated, you can do this: + /// + /// AstarPath.active.UpdateGraphs (obstacle.collider.bounds); + /// Where obstacle is the GameObject you just instantiated. + /// + /// As you can see, the UpdateGraphs function takes a Bounds parameter and it will send an update call to all updateable graphs. + /// + /// A grid graph will assume anything could have changed inside that bounding box, and recalculate all nodes that could possibly be affected. + /// Thus it may end up updating a few more nodes than just those covered by the bounding box. + /// + /// See: graph-updates (view in online documentation for working links) for more info about updating graphs during runtime + /// + /// Hexagonal graphs + /// The graph can be configured to work like a hexagon graph with some simple settings. The grid graph has a Shape dropdown. + /// If you set it to 'Hexagonal' the graph will behave as a hexagon graph. + /// Often you may want to rotate the graph +45 or -45 degrees. + /// [Open online documentation to see images] + /// + /// Note: Snapping to the closest node is not exactly as you would expect in a real hexagon graph, + /// but it is close enough that you will likely not notice. + /// + /// Configure using code + /// + /// A grid graph can be added and configured completely at runtime via code. + /// + /// + /// // This holds all graph data + /// AstarData data = AstarPath.active.data; + /// + /// // This creates a Grid Graph + /// GridGraph gg = data.AddGraph(typeof(GridGraph)) as GridGraph; + /// + /// // Setup a grid graph with some values + /// int width = 50; + /// int depth = 50; + /// float nodeSize = 1; + /// + /// gg.center = new Vector3(10, 0, 0); + /// + /// // Updates internal size from the above values + /// gg.SetDimensions(width, depth, nodeSize); + /// + /// // Scans all graphs + /// AstarPath.active.Scan(); + /// + /// + /// See: runtime-graphs (view in online documentation for working links) + /// + /// Tree colliders + /// It seems that Unity will only generate tree colliders at runtime when the game is started. + /// For this reason, the grid graph will not pick up tree colliders when outside of play mode + /// but it will pick them up once the game starts. If it still does not pick them up + /// make sure that the trees actually have colliders attached to them and that the tree prefabs are + /// in the correct layer (the layer should be included in the 'Collision Testing' mask). + /// + /// See: for documentation on the 'Height Testing' and 'Collision Testing' sections + /// of the grid graph settings. + /// See: + /// + [JsonOptIn] + [Pathfinding.Util.Preserve] + public class GridGraph : NavGraph, IUpdatableGraph, ITransformedGraph + , IRaycastableGraph { + protected override void DisposeUnmanagedData () { + // Destroy all nodes to make the graph go into an unscanned state + DestroyAllNodes(); + + // Clean up a reference in a static variable which otherwise should point to this graph forever and stop the GC from collecting it + GridNode.ClearGridGraph((int)graphIndex, this); + + // Dispose of native arrays. This is very important to avoid memory leaks! + rules.DisposeUnmanagedData(); + this.nodeData.Dispose(); + } + + protected override void DestroyAllNodes () { + GetNodes(node => { + // If the grid data happens to be invalid (e.g we had to abort a graph update while it was running) using 'false' as + // the parameter will prevent the Destroy method from potentially throwing IndexOutOfRange exceptions due to trying + // to access nodes outside the graph. It is safe to do this because we are destroying all nodes in the graph anyway. + // We do however need to clear custom connections in both directions + (node as GridNodeBase).ClearCustomConnections(true); + node.ClearConnections(false); + node.Destroy(); + }); + // Important: so that multiple calls to DestroyAllNodes still works + nodes = null; + } + + + /// + /// Number of layers in the graph. + /// For grid graphs this is always 1, for layered grid graphs it can be higher. + /// The nodes array has the size width*depth*layerCount. + /// + public virtual int LayerCount { + get => 1; + protected set { + if (value != 1) throw new System.NotSupportedException("Grid graphs cannot have multiple layers"); + } + } + + public virtual int MaxLayers => 1; + + public override int CountNodes () { + return nodes != null ? nodes.Length : 0; + } + + public override void GetNodes (System.Action action) { + if (nodes == null) return; + for (int i = 0; i < nodes.Length; i++) action(nodes[i]); + } + + /// + /// Determines the layout of the grid graph inspector in the Unity Editor. + /// + /// A grid graph can be set up as a normal grid, isometric grid or hexagonal grid. + /// Each of these modes use a slightly different inspector layout. + /// When changing the shape in the inspector, it will automatically set other relevant fields + /// to appropriate values. For example, when setting the shape to hexagonal it will automatically set + /// the field to Six. + /// + /// This field is only used in the editor, it has no effect on the rest of the game whatsoever. + /// + /// If you want to change the grid shape like in the inspector you can use the method. + /// + [JsonMember] + public InspectorGridMode inspectorGridMode = InspectorGridMode.Grid; + + /// + /// Determines how the size of each hexagon is set in the inspector. + /// For hexagons the normal nodeSize field doesn't really correspond to anything specific on the hexagon's geometry, so this enum is used to give the user the opportunity to adjust more concrete dimensions of the hexagons + /// without having to pull out a calculator to calculate all the square roots and complicated conversion factors. + /// + /// This field is only used in the graph inspector, the field will always use the same internal units. + /// If you want to set the node size through code then you can use . + /// + /// [Open online documentation to see images] + /// + /// See: + /// See: + /// See: + /// + [JsonMember] + public InspectorGridHexagonNodeSize inspectorHexagonSizeMode = InspectorGridHexagonNodeSize.Width; + + /// + /// Width of the grid in nodes. + /// + /// Grid graphs are typically anywhere from 10-500 nodes wide. But it can go up to 1024 nodes wide by default. + /// Consider using a recast graph instead, if you find yourself needing a very high resolution grid. + /// + /// This value will be clamped to at most 1024 unless ASTAR_LARGER_GRIDS has been enabled in the A* Inspector -> Optimizations tab. + /// + /// See: + /// See: SetDimensions + /// + public int width; + + /// + /// Depth (height) of the grid in nodes. + /// + /// Grid graphs are typically anywhere from 10-500 nodes wide. But it can go up to 1024 nodes wide by default. + /// Consider using a recast graph instead, if you find yourself needing a very high resolution grid. + /// + /// This value will be clamped to at most 1024 unless ASTAR_LARGER_GRIDS has been enabled in the A* Inspector -> Optimizations tab. + /// + /// See: + /// See: SetDimensions + /// + public int depth; + + /// + /// Scaling of the graph along the X axis. + /// This should be used if you want different scales on the X and Y axis of the grid + /// + /// This option is only visible in the inspector if the graph shape is set to isometric or advanced. + /// + [JsonMember] + public float aspectRatio = 1F; + + /// + /// Angle in degrees to use for the isometric projection. + /// If you are making a 2D isometric game, you may want to use this parameter to adjust the layout of the graph to match your game. + /// This will essentially scale the graph along one of its diagonals to produce something like this: + /// + /// A perspective view of an isometric graph. + /// [Open online documentation to see images] + /// + /// A top down view of an isometric graph. Note that the graph is entirely 2D, there is no perspective in this image. + /// [Open online documentation to see images] + /// + /// For commonly used values see and . + /// + /// Usually the angle that you want to use is either 30 degrees (alternatively 90-30 = 60 degrees) or atan(1/sqrt(2)) which is approximately 35.264 degrees (alternatively 90 - 35.264 = 54.736 degrees). + /// You might also want to rotate the graph plus or minus 45 degrees around the Y axis to get the oritientation required for your game. + /// + /// You can read more about it on the wikipedia page linked below. + /// + /// See: http://en.wikipedia.org/wiki/Isometric_projection + /// See: https://en.wikipedia.org/wiki/Isometric_graphics_in_video_games_and_pixel_art + /// See: rotation + /// + /// This option is only visible in the inspector if the graph shape is set to isometric or advanced. + /// + [JsonMember] + public float isometricAngle; + + /// Commonly used value for + public static readonly float StandardIsometricAngle = 90-Mathf.Atan(1/Mathf.Sqrt(2))*Mathf.Rad2Deg; + + /// Commonly used value for + public static readonly float StandardDimetricAngle = Mathf.Acos(1/2f)*Mathf.Rad2Deg; + + /// + /// If true, all edge costs will be set to the same value. + /// If false, diagonals will cost more. + /// This is useful for a hexagon graph where the diagonals are actually the same length as the + /// normal edges (since the graph has been skewed) + /// + [JsonMember] + public bool uniformEdgeCosts; + + /// + /// Rotation of the grid in degrees. + /// + /// The nodes are laid out along the X and Z axes of the rotation. + /// + /// For a 2D game, the rotation will typically be set to (-90, 270, 90). + /// If the graph is aligned with the XY plane, the inspector will automatically switch to 2D mode. + /// + /// See: + /// + [JsonMember] + public Vector3 rotation; + + /// + /// Center point of the grid in world space. + /// + /// The graph can be positioned anywhere in the world. + /// + /// See: + /// + [JsonMember] + public Vector3 center; + + /// Size of the grid. Can be negative or smaller than + [JsonMember] + public Vector2 unclampedSize = new Vector2(10, 10); + + /// + /// Size of one node in world units. + /// + /// For a grid layout, this is the length of the sides of the grid squares. + /// + /// For a hexagonal layout, this value does not correspond to any specific dimension of the hexagon. + /// Instead you can convert it to a dimension on a hexagon using . + /// + /// See: + /// + [JsonMember] + public float nodeSize = 1; + + /// Settings on how to check for walkability and height + [JsonMember] + public GraphCollision collision = new GraphCollision(); + + /// + /// The max y coordinate difference between two nodes to enable a connection. + /// Set to 0 to ignore the value. + /// + /// This affects for example how the graph is generated around ledges and stairs. + /// + /// See: + /// Version: Was previously called maxClimb + /// + [JsonMember] + public float maxStepHeight = 0.4F; + + /// + /// The max y coordinate difference between two nodes to enable a connection. + /// Deprecated: This field has been renamed to + /// + [System.Obsolete("This field has been renamed to maxStepHeight")] + public float maxClimb { + get { + return maxStepHeight; + } + set { + maxStepHeight = value; + } + } + + /// + /// Take the slope into account for . + /// + /// When this is enabled the normals of the terrain will be used to make more accurate estimates of how large the steps are between adjacent nodes. + /// + /// When this is disabled then calculated step between two nodes is their y coordinate difference. This may be inaccurate, especially at the start of steep slopes. + /// + /// [Open online documentation to see images] + /// + /// In the image below you can see an example of what happens near a ramp. + /// In the topmost image the ramp is not connected with the rest of the graph which is obviously not what we want. + /// In the middle image an attempt has been made to raise the max step height while keeping disabled. However this causes too many connections to be added. + /// The agent should not be able to go up the ramp from the side. + /// Finally in the bottommost image the has been restored to the original value but has been enabled. This configuration handles the ramp in a much smarter way. + /// Note that all the values in the image are just example values, they may be different for your scene. + /// [Open online documentation to see images] + /// + /// See: + /// + [JsonMember] + public bool maxStepUsesSlope = true; + + /// The max slope in degrees for a node to be walkable. + [JsonMember] + public float maxSlope = 90; + + /// + /// Use heigh raycasting normal for max slope calculation. + /// True if is less than 90 degrees. + /// + protected bool useRaycastNormal { get { return Math.Abs(90-maxSlope) > float.Epsilon; } } + + /// + /// Number of times to erode the graph. + /// + /// The graph can be eroded to add extra margin to obstacles. + /// It is very convenient if your graph contains ledges, and where the walkable nodes without erosion are too close to the edge. + /// + /// Below is an image showing a graph with 0, 1 and 2 erosion iterations: + /// [Open online documentation to see images] + /// + /// Note: A high number of erosion iterations can slow down graph updates during runtime. + /// This is because the region that is updated needs to be expanded by the erosion iterations times two to account for possible changes in the border nodes. + /// + /// See: erosionUseTags + /// + [JsonMember] + public int erodeIterations; + + /// + /// Use tags instead of walkability for erosion. + /// Tags will be used for erosion instead of marking nodes as unwalkable. The nodes will be marked with tags in an increasing order starting with the tag . + /// Debug with the Tags mode to see the effect. With this enabled you can in effect set how close different AIs are allowed to get to walls using the Valid Tags field on the Seeker component. + /// [Open online documentation to see images] + /// [Open online documentation to see images] + /// See: erosionFirstTag + /// + [JsonMember] + public bool erosionUseTags; + + /// + /// Tag to start from when using tags for erosion. + /// See: + /// See: + /// + [JsonMember] + public int erosionFirstTag = 1; + + /// + /// Bitmask for which tags can be overwritten by erosion tags. + /// + /// When is enabled, nodes near unwalkable nodes will be marked with tags. + /// However, if these nodes already have tags, you may want the custom tag to take precedence. + /// This mask controls which tags are allowed to be replaced by the new erosion tags. + /// + /// In the image below, erosion has applied tags which have overwritten both the base tag (tag 0) and the custom tag set on the nodes (shown in red). + /// [Open online documentation to see images] + /// + /// In the image below, erosion has applied tags, but it was not allowed to overwrite the custom tag set on the nodes (shown in red). + /// [Open online documentation to see images] + /// + /// See: + /// See: + /// See: This field is a bit mask. See: bitmasks (view in online documentation for working links) + /// + [JsonMember] + public int erosionTagsPrecedenceMask = -1; + + /// + /// Number of neighbours for each node. + /// Either four, six, eight connections per node. + /// + /// Six connections is primarily for hexagonal graphs. + /// + [JsonMember] + public NumNeighbours neighbours = NumNeighbours.Eight; + + /// + /// If disabled, will not cut corners on obstacles. + /// If this is true, and is set to Eight, obstacle corners are allowed to be cut by a connection. + /// + /// [Open online documentation to see images] + /// + [JsonMember] + public bool cutCorners = true; + + /// + /// Offset for the position when calculating penalty. + /// Deprecated: Use the RuleElevationPenalty class instead + /// See: penaltyPosition + /// + [JsonMember] + [System.Obsolete("Use the RuleElevationPenalty class instead")] + public float penaltyPositionOffset; + + /// + /// Use position (y-coordinate) to calculate penalty. + /// Deprecated: Use the RuleElevationPenalty class instead + /// + [JsonMember] + [System.Obsolete("Use the RuleElevationPenalty class instead")] + public bool penaltyPosition; + + /// + /// Scale factor for penalty when calculating from position. + /// Deprecated: Use the class instead + /// See: penaltyPosition + /// + [JsonMember] + [System.Obsolete("Use the RuleElevationPenalty class instead")] + public float penaltyPositionFactor = 1F; + + /// Deprecated: Use the class instead + [JsonMember] + [System.Obsolete("Use the RuleAnglePenalty class instead")] + public bool penaltyAngle; + + /// + /// How much penalty is applied depending on the slope of the terrain. + /// At a 90 degree slope (not that exactly 90 degree slopes can occur, but almost 90 degree), this penalty is applied. + /// At a 45 degree slope, half of this is applied and so on. + /// Note that you may require very large values, a value of 1000 is equivalent to the cost of moving 1 world unit. + /// + /// Deprecated: Use the class instead + /// + [JsonMember] + [System.Obsolete("Use the RuleAnglePenalty class instead")] + public float penaltyAngleFactor = 100F; + + /// + /// How much extra to penalize very steep angles. + /// + /// Deprecated: Use the class instead + /// + [JsonMember] + [System.Obsolete("Use the RuleAnglePenalty class instead")] + public float penaltyAnglePower = 1; + + /// + /// Additional rules to use when scanning the grid graph. + /// + /// + /// // Get the first grid graph in the scene + /// var gridGraph = AstarPath.active.data.gridGraph; + /// + /// gridGraph.rules.AddRule(new Pathfinding.Graphs.Grid.Rules.RuleAnglePenalty { + /// penaltyScale = 10000, + /// curve = AnimationCurve.Linear(0, 0, 90, 1), + /// }); + /// + /// + /// See: + /// See: + /// + [JsonMember] + public GridGraphRules rules = new GridGraphRules(); + + /// Show an outline of the grid nodes in the Unity Editor + [JsonMember] + public bool showMeshOutline = true; + + /// Show the connections between the grid nodes in the Unity Editor + [JsonMember] + public bool showNodeConnections; + + /// Show the surface of the graph. Each node will be drawn as a square (unless e.g hexagon graph mode has been enabled). + [JsonMember] + public bool showMeshSurface = true; + + /// + /// Holds settings for using a texture as source for a grid graph. + /// Texure data can be used for fine grained control over how the graph will look. + /// It can be used for positioning, penalty and walkability control. + /// Below is a screenshot of a grid graph with a penalty map applied. + /// It has the effect of the AI taking the longer path along the green (low penalty) areas. + /// [Open online documentation to see images] + /// Color data is got as 0...255 values. + /// + /// Warning: Can only be used with Unity 3.4 and up + /// + /// Deprecated: Use the RuleTexture class instead + /// + [JsonMember] + [System.Obsolete("Use the RuleTexture class instead")] + public TextureData textureData = new TextureData(); + + /// + /// Size of the grid. Will always be positive and larger than . + /// See: + /// + public Vector2 size { get; protected set; } + + /* End collision and stuff */ + + /// + /// Index offset to get neighbour nodes. Added to a node's index to get a neighbour node index. + /// + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + [System.NonSerialized] + public readonly int[] neighbourOffsets = new int[8]; + + /// + /// Costs to neighbour nodes. + /// + /// See . + /// + [System.NonSerialized] + public readonly uint[] neighbourCosts = new uint[8]; + + /// Offsets in the X direction for neighbour nodes. Only 1, 0 or -1 + public static readonly int[] neighbourXOffsets = { 0, 1, 0, -1, 1, 1, -1, -1 }; + + /// Offsets in the Z direction for neighbour nodes. Only 1, 0 or -1 + public static readonly int[] neighbourZOffsets = { -1, 0, 1, 0, -1, 1, 1, -1 }; + + /// Which neighbours are going to be used when =6 + internal static readonly int[] hexagonNeighbourIndices = { 0, 1, 5, 2, 3, 7 }; + + /// Which neighbours are going to be used when =4 + internal static readonly int[] axisAlignedNeighbourIndices = { 0, 1, 2, 3 }; + + /// Which neighbours are going to be used when =8 + internal static readonly int[] allNeighbourIndices = { 0, 1, 2, 3, 4, 5, 6, 7 }; + + /// + /// Neighbour direction indices to use depending on how many neighbours each node should have. + /// + /// The following illustration shows the direction indices for all 8 neighbours, + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// For other neighbour counts, a subset of these will be returned. + /// + /// These can then be used to index into the , , , and arrays. + /// + /// See: + /// See: + /// + public static int[] GetNeighbourDirections (NumNeighbours neighbours) { + switch (neighbours) { + case NumNeighbours.Four: + return axisAlignedNeighbourIndices; + case NumNeighbours.Six: + return hexagonNeighbourIndices; + default: + return allNeighbourIndices; + } + } + + /// + /// Mask based on hexagonNeighbourIndices. + /// This indicates which connections (out of the 8 standard ones) should be enabled for hexagonal graphs. + /// + /// + /// int hexagonConnectionMask = 0; + /// for (int i = 0; i < GridGraph.hexagonNeighbourIndices.Length; i++) hexagonConnectionMask |= 1 << GridGraph.hexagonNeighbourIndices[i]; + /// + /// + internal const int HexagonConnectionMask = 0b010101111; + + /// + /// All nodes in this graph. + /// Nodes are laid out row by row. + /// + /// The first node has grid coordinates X=0, Z=0, the second one X=1, Z=0 + /// the last one has grid coordinates X=width-1, Z=depth-1. + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// int x = 5; + /// int z = 8; + /// GridNodeBase node = gg.nodes[z*gg.width + x]; + /// + /// + /// See: + /// See: + /// + public GridNodeBase[] nodes; + + /// + /// Internal data for each node. + /// + /// It also contains some data not stored in the node objects, such as normals for the surface of the graph. + /// These normals need to be saved when the option is enabled for graph updates to work. + /// + protected GridGraphNodeData nodeData; + + internal ref GridGraphNodeData nodeDataRef => ref nodeData; + + /// + /// Determines how the graph transforms graph space to world space. + /// See: + /// + public GraphTransform transform { get; private set; } = new GraphTransform(Matrix4x4.identity); + + /// + /// Delegate which creates and returns a single instance of the node type for this graph. + /// This may be set in the constructor for graphs inheriting from the GridGraph to change the node type of the graph. + /// + protected System.Func newGridNodeDelegate = () => new GridNode(); + + /// + /// Get or set if the graph should be in 2D mode. + /// + /// Note: This is just a convenience property, this property will actually read/modify the of the graph. A rotation aligned with the 2D plane is what determines if the graph is 2D or not. + /// + /// See: You can also set if the graph should use 2D physics using `this.collision.use2D` (). + /// + public bool is2D { + get { + return Quaternion.Euler(this.rotation) * Vector3.up == -Vector3.forward; + } + set { + if (value != is2D) { + this.rotation = value ? new Vector3(this.rotation.y - 90, 270, 90) : new Vector3(0, this.rotation.x + 90, 0); + } + } + } + + public override bool isScanned => nodes != null; + + protected virtual GridNodeBase[] AllocateNodesJob (int size, out JobHandle dependency) { + var newNodes = new GridNodeBase[size]; + + dependency = active.AllocateNodes(newNodes, size, newGridNodeDelegate, 1); + return newNodes; + } + + /// Used for using a texture as a source for a grid graph. + public class TextureData { + public bool enabled; + public Texture2D source; + public float[] factors = new float[3]; + public ChannelUse[] channels = new ChannelUse[3]; + + Color32[] data; + + /// Reads texture data + public void Initialize () { + if (enabled && source != null) { + for (int i = 0; i < channels.Length; i++) { + if (channels[i] != ChannelUse.None) { + try { + data = source.GetPixels32(); + } catch (UnityException e) { + Debug.LogWarning(e.ToString()); + data = null; + } + break; + } + } + } + } + + /// Applies the texture to the node + public void Apply (GridNode node, int x, int z) { + if (enabled && data != null && x < source.width && z < source.height) { + Color32 col = data[z*source.width+x]; + + if (channels[0] != ChannelUse.None) { + ApplyChannel(node, x, z, col.r, channels[0], factors[0]); + } + + if (channels[1] != ChannelUse.None) { + ApplyChannel(node, x, z, col.g, channels[1], factors[1]); + } + + if (channels[2] != ChannelUse.None) { + ApplyChannel(node, x, z, col.b, channels[2], factors[2]); + } + + node.WalkableErosion = node.Walkable; + } + } + + /// Applies a value to the node using the specified ChannelUse + void ApplyChannel (GridNode node, int x, int z, int value, ChannelUse channelUse, float factor) { + switch (channelUse) { + case ChannelUse.Penalty: + node.Penalty += (uint)Mathf.RoundToInt(value*factor); + break; + case ChannelUse.Position: + node.position = GridNode.GetGridGraph(node.GraphIndex).GraphPointToWorld(x, z, value); + break; + case ChannelUse.WalkablePenalty: + if (value == 0) { + node.Walkable = false; + } else { + node.Penalty += (uint)Mathf.RoundToInt((value-1)*factor); + } + break; + } + } + + public enum ChannelUse { + None, + Penalty, + Position, + WalkablePenalty, + } + } + + public override void RelocateNodes (Matrix4x4 deltaMatrix) { + // It just makes a lot more sense to use the other overload and for that case we don't have to serialize the matrix + throw new System.Exception("This method cannot be used for Grid Graphs. Please use the other overload of RelocateNodes instead"); + } + + /// + /// Relocate the grid graph using new settings. + /// This will move all nodes in the graph to new positions which matches the new settings. + /// + /// + /// // Move the graph to the origin, with no rotation, and with a node size of 1.0 + /// var gg = AstarPath.active.data.gridGraph; + /// gg.RelocateNodes(center: Vector3.zero, rotation: Quaternion.identity, nodeSize: 1.0f); + /// + /// + public void RelocateNodes (Vector3 center, Quaternion rotation, float nodeSize, float aspectRatio = 1, float isometricAngle = 0) { + var previousTransform = transform; + + this.center = center; + this.rotation = rotation.eulerAngles; + this.aspectRatio = aspectRatio; + this.isometricAngle = isometricAngle; + + DirtyBounds(bounds); + SetDimensions(width, depth, nodeSize); + + GetNodes(node => { + var gnode = node as GridNodeBase; + var height = previousTransform.InverseTransform((Vector3)node.position).y; + node.position = GraphPointToWorld(gnode.XCoordinateInGrid, gnode.ZCoordinateInGrid, height); + }); + DirtyBounds(bounds); + } + + /// + /// True if the point is inside the bounding box of this graph. + /// + /// For a graph that uses 2D physics, or if height testing is disabled, then the graph is treated as infinitely tall. + /// Otherwise, the height of the graph is determined by . + /// + /// Note: For an unscanned graph, this will always return false. + /// + public override bool IsInsideBounds (Vector3 point) { + if (this.nodes == null) return false; + + var local = transform.InverseTransform(point); + if (!(local.x >= 0 && local.z >= 0 && local.x <= width && local.z <= depth)) return false; + + if (collision.use2D || !collision.heightCheck) return true; + + return local.y >= 0 && local.y <= collision.fromHeight; + } + + /// + /// World bounding box for the graph. + /// + /// This always contains the whole graph. + /// + /// Note: Since this is an axis-aligned bounding box, it may not be particularly tight if the graph is significantly rotated. + /// + public override Bounds bounds => transform.Transform(new Bounds(new Vector3(width*0.5f, collision.fromHeight*0.5f, depth*0.5f), new Vector3(width, collision.fromHeight, depth))); + + /// + /// Transform a point in graph space to world space. + /// This will give you the node position for the node at the given x and z coordinate + /// if it is at the specified height above the base of the graph. + /// + public Int3 GraphPointToWorld (int x, int z, float height) { + return (Int3)transform.Transform(new Vector3(x+0.5f, height, z+0.5f)); + } + + /// + /// Converts a hexagon dimension to a node size. + /// + /// A hexagon can be defined using either its diameter, or width, none of which are the same as the used internally to define the size of a single node. + /// + /// See: + /// + public static float ConvertHexagonSizeToNodeSize (InspectorGridHexagonNodeSize mode, float value) { + if (mode == InspectorGridHexagonNodeSize.Diameter) value *= 1.5f/(float)System.Math.Sqrt(2.0f); + else if (mode == InspectorGridHexagonNodeSize.Width) value *= (float)System.Math.Sqrt(3.0f/2.0f); + return value; + } + + /// + /// Converts an internal node size to a hexagon dimension. + /// + /// A hexagon can be defined using either its diameter, or width, none of which are the same as the used internally to define the size of a single node. + /// + /// See: ConvertHexagonSizeToNodeSize + /// + public static float ConvertNodeSizeToHexagonSize (InspectorGridHexagonNodeSize mode, float value) { + if (mode == InspectorGridHexagonNodeSize.Diameter) value *= (float)System.Math.Sqrt(2.0f)/1.5f; + else if (mode == InspectorGridHexagonNodeSize.Width) value *= (float)System.Math.Sqrt(2.0f/3.0f); + return value; + } + + public int Width { + get { + return width; + } + set { + width = value; + } + } + public int Depth { + get { + return depth; + } + set { + depth = value; + } + } + + /// + /// Default cost of moving one node in a particular direction. + /// + /// Note: You can only call this after the graph has been scanned. Otherwise it will return zero. + /// + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + public uint GetConnectionCost (int dir) { + return neighbourCosts[dir]; + } + + /// + /// Changes the grid shape. + /// This is equivalent to changing the 'shape' dropdown in the grid graph inspector. + /// + /// Calling this method will set , , and + /// to appropriate values for that shape. + /// + /// Note: Setting the shape to does not do anything except set the field. + /// + /// See: + /// + public void SetGridShape (InspectorGridMode shape) { + switch (shape) { + case InspectorGridMode.Grid: + isometricAngle = 0; + aspectRatio = 1; + uniformEdgeCosts = false; + if (neighbours == NumNeighbours.Six) neighbours = NumNeighbours.Eight; + break; + case InspectorGridMode.Hexagonal: + isometricAngle = StandardIsometricAngle; + aspectRatio = 1; + uniformEdgeCosts = true; + neighbours = NumNeighbours.Six; + break; + case InspectorGridMode.IsometricGrid: + uniformEdgeCosts = false; + if (neighbours == NumNeighbours.Six) neighbours = NumNeighbours.Eight; + isometricAngle = StandardIsometricAngle; + break; + case InspectorGridMode.Advanced: + default: + break; + } + inspectorGridMode = shape; + } + + /// + /// Aligns this grid to a given tilemap or grid layout. + /// + /// This is very handy if your game uses a tilemap for rendering and you want to make sure the graph is laid out exactly the same. + /// Matching grid parameters manually can be quite tricky in some cases. + /// + /// The inspector will automatically show a button to align to a tilemap if one is detected in the scene. + /// If no tilemap is detected, the button be hidden. + /// + /// [Open online documentation to see images] + /// + /// See: tilemaps (view in online documentation for working links) + /// + public void AlignToTilemap (UnityEngine.GridLayout grid) { + var origin = grid.CellToWorld(new Vector3Int(0, 0, 0)); + var dx = grid.CellToWorld(new Vector3Int(1, 0, 0)) - origin; + var dy = grid.CellToWorld(new Vector3Int(0, 1, 0)) - origin; + + switch (grid.cellLayout) { + case GridLayout.CellLayout.Rectangle: { + var rot = new quaternion(new float3x3( + dx.normalized, + -Vector3.Cross(dx, dy).normalized, + dy.normalized + )); + + this.nodeSize = dy.magnitude; + this.isometricAngle = 0f; + this.aspectRatio = dx.magnitude / this.nodeSize; + if (!float.IsFinite(this.aspectRatio)) this.aspectRatio = 1.0f; + this.rotation = ((Quaternion)rot).eulerAngles; + this.uniformEdgeCosts = false; + if (this.neighbours == NumNeighbours.Six) this.neighbours = NumNeighbours.Eight; + this.inspectorGridMode = InspectorGridMode.Grid; + break; + } + case GridLayout.CellLayout.Isometric: + var d1 = grid.CellToWorld(new Vector3Int(1, 1, 0)) - origin; + var d2 = grid.CellToWorld(new Vector3Int(1, -1, 0)) - origin; + if (d1.magnitude > d2.magnitude) { + Memory.Swap(ref d1, ref d2); + } + var rot2 = math.mul(new quaternion(new float3x3( + d2.normalized, + -Vector3.Cross(d2, d1).normalized, + d1.normalized + )), quaternion.RotateY(-math.PI * 0.25f)); + + this.isometricAngle = Mathf.Acos(d1.magnitude / d2.magnitude) * Mathf.Rad2Deg; + this.nodeSize = d2.magnitude / Mathf.Sqrt(2.0f); + this.rotation = ((Quaternion)rot2).eulerAngles; + this.uniformEdgeCosts = false; + this.aspectRatio = 1.0f; + if (this.neighbours == NumNeighbours.Six) this.neighbours = NumNeighbours.Eight; + this.inspectorGridMode = InspectorGridMode.IsometricGrid; + break; + case GridLayout.CellLayout.Hexagon: + // Note: Unity does not use a mathematically perfect hexagonal layout by default. The cells can be squished vertically or horizontally. + var d12 = grid.CellToWorld(new Vector3Int(1, 0, 0)) - origin; + var d32 = grid.CellToWorld(new Vector3Int(-1, 1, 0)) - origin; + this.aspectRatio = (d12.magnitude / Mathf.Sqrt(2f/3f)) / (Vector3.Cross(d12.normalized, d32).magnitude / (1.5f * Mathf.Sqrt(2)/3f)); + this.nodeSize = GridGraph.ConvertHexagonSizeToNodeSize(InspectorGridHexagonNodeSize.Width, d12.magnitude / aspectRatio); + + var crossAxis = -Vector3.Cross(d12, Vector3.Cross(d12, d32)); + + var rot3 = new quaternion(new float3x3( + d12.normalized, + -Vector3.Cross(d12, crossAxis).normalized, + crossAxis.normalized + )); + + this.rotation = ((Quaternion)rot3).eulerAngles; + this.uniformEdgeCosts = true; + this.neighbours = NumNeighbours.Six; + this.inspectorGridMode = InspectorGridMode.Hexagonal; + break; + } + + // Snap center to the closest grid point + UpdateTransform(); + var layoutCellPivotIsCenter = grid.cellLayout == GridLayout.CellLayout.Hexagon; + var offset = new Vector3(((width % 2) == 0) != layoutCellPivotIsCenter ? 0 : 0.5f, 0, ((depth % 2) == 0) != layoutCellPivotIsCenter ? 0f : 0.5f); + var worldOffset = transform.TransformVector(offset); + var centerCell = grid.WorldToCell(center + worldOffset); + centerCell.z = 0; + center = grid.CellToWorld(centerCell) - worldOffset; + if (float.IsNaN(center.x)) center = Vector3.zero; + UpdateTransform(); + } + + /// + /// Updates from , and values. + /// Also . + /// Note: This does not rescan the graph, that must be done with Scan + /// + /// You should use this method instead of setting the and fields + /// as the grid dimensions are not defined by the and variables but by + /// the and variables. + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// var width = 80; + /// var depth = 60; + /// var nodeSize = 1.0f; + /// + /// gg.SetDimensions(width, depth, nodeSize); + /// + /// // Recalculate the graph + /// AstarPath.active.Scan(); + /// + /// + public void SetDimensions (int width, int depth, float nodeSize) { + unclampedSize = new Vector2(width, depth)*nodeSize; + this.nodeSize = nodeSize; + UpdateTransform(); + } + + /// + /// Updates the field which transforms graph space to world space. + /// In graph space all nodes are laid out in the XZ plane with the first node having a corner in the origin. + /// One unit in graph space is one node so the first node in the graph is at (0.5,0) the second one at (1.5,0) etc. + /// + /// This takes the current values of the parameters such as position and rotation into account. + /// The transform that was used the last time the graph was scanned is stored in the field. + /// + /// The field is calculated using this method when the graph is scanned. + /// The width, depth variables are also updated based on the field. + /// + public void UpdateTransform () { + CalculateDimensions(out width, out depth, out nodeSize); + transform = CalculateTransform(); + } + + /// + /// Returns a new transform which transforms graph space to world space. + /// Does not update the field. + /// See: + /// + public GraphTransform CalculateTransform () { + CalculateDimensions(out var newWidth, out var newDepth, out var newNodeSize); + + if (this.neighbours == NumNeighbours.Six) { + var ax1 = new Vector3(newNodeSize*aspectRatio*Mathf.Sqrt(2f/3f), 0, 0); + var ax2 = new Vector3(0, 1, 0); + var ax3 = new Vector3(-aspectRatio * newNodeSize * 0.5f * Mathf.Sqrt(2f/3f), 0, newNodeSize * (1.5f * Mathf.Sqrt(2)/3f)); + var m = new Matrix4x4( + (Vector4)ax1, + (Vector4)ax2, + (Vector4)ax3, + new Vector4(0, 0, 0, 1) + ); + + var boundsMatrix = Matrix4x4.TRS(center, Quaternion.Euler(rotation), Vector3.one) * m; + + // Generate a matrix where Vector3.zero is the corner of the graph instead of the center + m = Matrix4x4.TRS(boundsMatrix.MultiplyPoint3x4(-new Vector3(newWidth, 0, newDepth)*0.5F), Quaternion.Euler(rotation), Vector3.one) * m; + return new GraphTransform(m); + } else { + // Generate a matrix which shrinks the graph along the main diagonal + var squishFactor = new Vector3(Mathf.Cos(Mathf.Deg2Rad*isometricAngle), 1, 1); + var isometricMatrix = Matrix4x4.Scale(new Vector3(newNodeSize*aspectRatio, 1, newNodeSize)); + var squishAngle = Mathf.Atan2(newNodeSize, newNodeSize*aspectRatio) * Mathf.Rad2Deg; + isometricMatrix = Matrix4x4.Rotate(Quaternion.Euler(0, -squishAngle, 0)) * Matrix4x4.Scale(squishFactor) * Matrix4x4.Rotate(Quaternion.Euler(0, squishAngle, 0)) * isometricMatrix; + + // Generate a matrix for the bounds of the graph + // This moves a point to the correct offset in the world and the correct rotation and the aspect ratio and isometric angle is taken into account + var boundsMatrix = Matrix4x4.TRS(center, Quaternion.Euler(rotation), Vector3.one) * isometricMatrix; + + // Generate a matrix where Vector3.zero is the corner of the graph instead of the center + // The unit is nodes here (so (0.5,0,0.5) is the position of the first node and (1.5,0,0.5) is the position of the second node) + // 0.5 is added since this is the node center, not its corner. In graph space a node has a size of 1 + var m = Matrix4x4.TRS(boundsMatrix.MultiplyPoint3x4(-new Vector3(newWidth, 0, newDepth)*0.5F), Quaternion.Euler(rotation), Vector3.one) * isometricMatrix; + + return new GraphTransform(m); + } + } + + /// + /// Calculates the width/depth of the graph from and . + /// The node size may be changed due to constraints that the width/depth is not + /// allowed to be larger than 1024 (artificial limit). + /// + void CalculateDimensions (out int width, out int depth, out float nodeSize) { + var newSize = unclampedSize; + + // Make sure size is positive + newSize.x *= Mathf.Sign(newSize.x); + newSize.y *= Mathf.Sign(newSize.y); + +#if !ASTAR_LARGER_GRIDS + // Clamp the nodeSize so that the graph is never larger than 1024*1024 + nodeSize = Mathf.Max(this.nodeSize, newSize.x/1024f); + nodeSize = Mathf.Max(this.nodeSize, newSize.y/1024f); +#else + nodeSize = Mathf.Max(this.nodeSize, newSize.x/8192f); + nodeSize = Mathf.Max(this.nodeSize, newSize.y/8192f); +#endif + + // Prevent the graph to become smaller than a single node + newSize.x = newSize.x < nodeSize ? nodeSize : newSize.x; + newSize.y = newSize.y < nodeSize ? nodeSize : newSize.y; + + size = newSize; + + // Calculate the number of nodes along each side + width = Mathf.FloorToInt(size.x / nodeSize); + depth = Mathf.FloorToInt(size.y / nodeSize); + + // Take care of numerical edge cases + if (Mathf.Approximately(size.x / nodeSize, Mathf.CeilToInt(size.x / nodeSize))) { + width = Mathf.CeilToInt(size.x / nodeSize); + } + + if (Mathf.Approximately(size.y / nodeSize, Mathf.CeilToInt(size.y / nodeSize))) { + depth = Mathf.CeilToInt(size.y / nodeSize); + } + } + + public override float NearestNodeDistanceSqrLowerBound (Vector3 position, NNConstraint constraint) { + if (nodes == null || depth*width*LayerCount != nodes.Length) { + return float.PositiveInfinity; + } + + position = transform.InverseTransform(position); + + float xf = position.x; + float zf = position.z; + float xc = Mathf.Clamp(xf, 0, width); + float zc = Mathf.Clamp(zf, 0, depth); + + // Node y coordinates (in graph space) may range from -inf to +inf theoretically, so we only use the xz distance to calculate the lower bound + return (xf-xc)*(xf-xc) + (zf-zc)*(zf-zc); + } + + protected virtual GridNodeBase GetNearestFromGraphSpace (Vector3 positionGraphSpace) { + if (nodes == null || depth*width != nodes.Length) { + return null; + } + + float xf = positionGraphSpace.x; + float zf = positionGraphSpace.z; + int x = Mathf.Clamp((int)xf, 0, width-1); + int z = Mathf.Clamp((int)zf, 0, depth-1); + return nodes[z*width+x]; + } + + public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, float maxDistanceSqr) { + if (nodes == null || depth*width*LayerCount != nodes.Length) { + return NNInfo.Empty; + } + + // Position in global space + Vector3 globalPosition = position; + + // Position in graph space + position = transform.InverseTransform(position); + + // Find the coordinates of the closest node + float xf = position.x; + float zf = position.z; + int x = Mathf.Clamp((int)xf, 0, width-1); + int z = Mathf.Clamp((int)zf, 0, depth-1); + + GridNodeBase minNode = null; + + // If set, we use another distance metric instead of the normal euclidean distance. + // See constraint.projectionAxis for more info. + // Note: The grid graph does not support any projectionAxis other than one parallel to the graph's up axis. + // So if the constraint has a projectionAxis, we treat it as if it is transform.up + var projectedDistance = constraint != null ? constraint.distanceMetric.isProjectedDistance : false; + + // Search up to this distance + float minDistSqr = maxDistanceSqr; + var layerCount = LayerCount; + var layerStride = width*depth; + long yOffset = 0; + float yDistanceScale = 0; + Int3 up = default; + if (projectedDistance) { + up = (Int3)transform.WorldUpAtGraphPosition(globalPosition); + yOffset = Int3.DotLong((Int3)globalPosition, up); + yDistanceScale = constraint.distanceMetric.distanceScaleAlongProjectionDirection * Int3.PrecisionFactor * Int3.PrecisionFactor; + } + + // Check the closest cell + for (int y = 0; y < layerCount; y++) { + var node = nodes[z*width + x + layerStride*y]; + if (node != null && (constraint == null || constraint.Suitable(node))) { + float cost; + if (projectedDistance) { + var distX = math.clamp(xf, x, x + 1.0f) - xf; + var distZ = math.clamp(zf, z, z + 1.0f) - zf; + var distSideSqr = nodeSize*nodeSize * (distX*distX + distZ*distZ); + var distUp = (Int3.DotLong(node.position, up) - yOffset) * yDistanceScale; + cost = Mathf.Sqrt(distSideSqr) + Mathf.Abs(distUp); + cost = cost*cost; + } else { + cost = ((Vector3)node.position-globalPosition).sqrMagnitude; + } + if (cost <= minDistSqr) { + // Minimum distance so far + minDistSqr = cost; + minNode = node; + } + } + } + + // Search in a square/spiral pattern around the closest cell + // + // 6 + // 7 1 5 + // 8 2 X 0 4 + // 9 3 . + // . + // + // and so on... + + // Lower bound on the distance to any cell which is not the closest one + float distanceToEdgeOfNode = Mathf.Min(Mathf.Min(xf - x, 1.0f - (xf - x)), Mathf.Min(zf - z, 1.0f - (zf - z))) * nodeSize; + + for (int w = 1;; w++) { + // Check if the nodes are within distance limit. + // This is an optimization to avoid calculating the distance to all nodes. + // Since we search in a square pattern, we will have to search up to + // sqrt(2) times further away than the closest node we have found so far (or the maximum distance). + var distanceThreshold = math.max(0, w-2)*nodeSize + distanceToEdgeOfNode; + if (minDistSqr - 0.00001f <= distanceThreshold*distanceThreshold) { + break; + } + + bool anyInside = false; + + int nx = x + w; + int nz = z; + int dx = -1; + int dz = 1; + for (int d = 0; d < 4; d++) { + for (int i = 0; i < w; i++) { + if (nx >= 0 && nz >= 0 && nx < width && nz < depth) { + anyInside = true; + var nodeIndex = nx+nz*width; + for (int y = 0; y < layerCount; y++) { + var node = nodes[nodeIndex + layerStride*y]; + if (node != null && (constraint == null || constraint.Suitable(node))) { + float cost; + if (projectedDistance) { + var distX = math.clamp(xf, nx, nx + 1.0f) - xf; + var distZ = math.clamp(zf, nz, nz + 1.0f) - zf; + var distSideSqr = nodeSize*nodeSize * (distX*distX + distZ*distZ); + var distUp = (Int3.DotLong(node.position, up) - yOffset) * yDistanceScale; + cost = Mathf.Sqrt(distSideSqr) + Mathf.Abs(distUp); + cost = cost*cost; + } else { + cost = ((Vector3)node.position-globalPosition).sqrMagnitude; + } + if (cost <= minDistSqr) { + // Minimum distance so far + minDistSqr = cost; + minNode = node; + } + } + } + } + nx += dx; + nz += dz; + } + + // Rotate direction by 90 degrees counter-clockwise + var ndx = -dz; + var ndz = dx; + dx = ndx; + dz = ndz; + } + + // No nodes were inside grid bounds + // We will not be able to find any more valid nodes + // so just break + if (!anyInside) break; + } + + if (minNode != null) { + if (projectedDistance) { + // Walk towards the closest cell. + // We do this to ensure that if projectedDistance is true, then internal edges in the graph + // will *never* be obstructions for the agent. + // + // For example, if we have two nodes A and B which have different Y coordinates, + // and we have an agent (X) which has just stepped out of A and into node B. + // Assume that A and B are connected. + // + // __A__X + // + // __B__ + // + // In this case, even though A might be closer with DistanceMetric.ClosestAsSeenFromAboveSoft, + // we want to return node B because clamping to A would mean clamping along to an obstacle edge + // which does not exist (A and B are connected). + // This is very important when this is used to clamp the agent to the navmesh, + // but it is also generally what you want in other situations as well. + while (true) { + var dx = x - minNode.XCoordinateInGrid; + var dz = z - minNode.ZCoordinateInGrid; + if (dx == 0 && dz == 0) break; + var d1 = dx > 0 ? 1 : (dx < 0 ? 3 : -1); + var d2 = dz > 0 ? 2 : (dz < 0 ? 0 : -1); + if (Mathf.Abs(dx) < Mathf.Abs(dz)) Memory.Swap(ref d1, ref d2); + + // Try to walk along d1, if that does not work, try d2 + var next = minNode.GetNeighbourAlongDirection(d1); + if (next != null && (constraint == null || constraint.Suitable(next))) minNode = next; + else if (d2 != -1 && (next = minNode.GetNeighbourAlongDirection(d2)) != null && (constraint == null || constraint.Suitable(next))) minNode = next; + else break; + } + } + + // Closest point on the node if the node is treated as a square + var nx = minNode.XCoordinateInGrid; + var nz = minNode.ZCoordinateInGrid; + var closest = transform.Transform(new Vector3(Mathf.Clamp(xf, nx, nx+1f), transform.InverseTransform((Vector3)minNode.position).y, Mathf.Clamp(zf, nz, nz+1f))); + // If projectedDistance is enabled, the distance is already accurate. + // Otherwise, we need to calculate the distance to the closest point on the node instead of to the center + var cost = projectedDistance ? minDistSqr : (closest-globalPosition).sqrMagnitude; + return cost <= maxDistanceSqr ? new NNInfo( + minNode, + closest, + cost + ) : NNInfo.Empty; + } else { + return NNInfo.Empty; + } + } + + /// + /// Sets up with the current settings. , , and are set up. + /// The cost for a non-diagonal movement between two adjacent nodes is RoundToInt ( * Int3.Precision) + /// The cost for a diagonal movement between two adjacent nodes is RoundToInt ( * Sqrt (2) * Int3.Precision) + /// + public virtual void SetUpOffsetsAndCosts () { + // First 4 are for the four directly adjacent nodes the last 4 are for the diagonals + neighbourOffsets[0] = -width; + neighbourOffsets[1] = 1; + neighbourOffsets[2] = width; + neighbourOffsets[3] = -1; + neighbourOffsets[4] = -width+1; + neighbourOffsets[5] = width+1; + neighbourOffsets[6] = width-1; + neighbourOffsets[7] = -width-1; + + // The width of a single node, and thus also the distance between two adjacent nodes (axis aligned). + // For hexagonal graphs the node size is different from the width of a hexaon. + float nodeWidth = neighbours == NumNeighbours.Six ? ConvertNodeSizeToHexagonSize(InspectorGridHexagonNodeSize.Width, nodeSize) : nodeSize; + + uint straightCost = (uint)Mathf.RoundToInt(nodeWidth*Int3.Precision); + + // Diagonals normally cost sqrt(2) (approx 1.41) times more + uint diagonalCost = uniformEdgeCosts ? straightCost : (uint)Mathf.RoundToInt(nodeWidth*Mathf.Sqrt(2F)*Int3.Precision); + + neighbourCosts[0] = straightCost; + neighbourCosts[1] = straightCost; + neighbourCosts[2] = straightCost; + neighbourCosts[3] = straightCost; + neighbourCosts[4] = diagonalCost; + neighbourCosts[5] = diagonalCost; + neighbourCosts[6] = diagonalCost; + neighbourCosts[7] = diagonalCost; + + /* Z + * | + * | + * + * 6 2 5 + * \ | / + * -- 3 - X - 1 ----- X + * / | \ + * 7 0 4 + * + * | + * | + */ + } + + public enum RecalculationMode { + /// Recalculates the nodes from scratch. Used when the graph is first scanned. You should have destroyed all existing nodes before updating the graph with this mode. + RecalculateFromScratch, + /// Recalculate the minimal number of nodes necessary to guarantee changes inside the graph update's bounding box are taken into account. Some data may be read from the existing nodes + RecalculateMinimal, + /// Nodes are not recalculated. Used for graph updates which only set node properties + NoRecalculation, + } + + /// + /// Moves the grid by a number of nodes. + /// + /// This is used by the component to efficiently move the graph. + /// + /// All nodes that can stay in the same position will stay. The ones that would have fallen off the edge of the graph will wrap around to the other side + /// and then be recalculated. + /// + /// See: + /// + /// Returns: An async graph update promise. See . + /// + /// Number of nodes along the graph's X axis to move by. + /// Number of nodes along the graph's Z axis to move by. + public IGraphUpdatePromise TranslateInDirection(int dx, int dz) => new GridGraphMovePromise(this, dx, dz); + + class GridGraphMovePromise : IGraphUpdatePromise { + public GridGraph graph; + public int dx; + public int dz; + IGraphUpdatePromise[] promises; + IntRect[] rects; + int3 startingSize; + + static void DecomposeInsetsToRectangles (int width, int height, int insetLeft, int insetRight, int insetBottom, int insetTop, IntRect[] output) { + output[0] = new IntRect(0, 0, insetLeft - 1, height - 1); + output[1] = new IntRect(width - insetRight, 0, width - 1, height - 1); + output[2] = new IntRect(insetLeft, 0, width - insetRight - 1, insetBottom - 1); + output[3] = new IntRect(insetLeft, height - insetTop - 1, width - insetRight - 1, height - 1); + } + + public GridGraphMovePromise(GridGraph graph, int dx, int dz) { + this.graph = graph; + this.dx = dx; + this.dz = dz; + var transform = graph.transform * Matrix4x4.Translate(new Vector3(dx, 0, dz)); + + // If the graph is moved by more than half its width/depth, then we recalculate the whole graph instead + startingSize = new int3(graph.width, graph.LayerCount, graph.depth); + if (math.abs(dx) > graph.width/2 || math.abs(dz) > graph.depth/2) { + rects = new IntRect[1] { + new IntRect(0, 0, graph.width - 1, graph.depth - 1) + }; + } else { + // We recalculate nodes within some distance from each side of the (translated) grid. + // We must always recalculate at least the nodes along the border, since they may have had + // connections to nodes that are now outside the graph. + // TODO: This can potentially be optimized to just clearing the out-of-bounds connections + // on border nodes, instead of completely recalculating the border nodes. + var insetLeft = math.max(1, -dx); + var insetRight = math.max(1, dx); + var insetBottom = math.max(1, -dz); + var insetTop = math.max(1, dz); + rects = new IntRect[4]; + DecomposeInsetsToRectangles(graph.width, graph.depth, insetLeft, insetRight, insetBottom, insetTop, rects); + } + + promises = new GridGraphUpdatePromise[rects.Length]; + var nodes = new GridGraphUpdatePromise.NodesHolder { nodes = graph.nodes }; + for (int i = 0; i < rects.Length; i++) { + var dependencyTracker = ObjectPool.Claim(); + // TODO: Use the exact rect given, don't expand it using physics checks + // We do need to expand the insets using erosion, though. + promises[i] = new GridGraphUpdatePromise( + graph: graph, + transform: transform, + nodes: nodes, + nodeArrayBounds: startingSize, + rect: rects[i], + dependencyTracker: dependencyTracker, + nodesDependsOn: default, + allocationMethod: Allocator.Persistent, + recalculationMode: RecalculationMode.RecalculateMinimal, + graphUpdateObject: null, + ownsJobDependencyTracker: true + ); + } + } + + public IEnumerator Prepare () { + yield return graph.nodeData.Rotate2D(-dx, -dz, default); + + for (int i = 0; i < promises.Length; i++) { + var it = promises[i].Prepare(); + while (it.MoveNext()) yield return it.Current; + } + } + + public void Apply (IGraphUpdateContext ctx) { + graph.AssertSafeToUpdateGraph(); + var nodes = graph.nodes; + if (!math.all(new int3(graph.width, graph.LayerCount, graph.depth) == startingSize)) throw new System.InvalidOperationException("The graph has been resized since the update was created. This is not allowed."); + if (nodes == null || nodes.Length != graph.width * graph.depth * graph.LayerCount) { + throw new System.InvalidOperationException("The Grid Graph is not scanned, cannot recalculate connections."); + } + + Profiler.BeginSample("Rotating node array"); + Memory.Rotate3DArray(nodes, startingSize, -dx, -dz); + Profiler.EndSample(); + + Profiler.BeginSample("Recalculating node indices"); + // Recalculate the node indices for all nodes that exist before the update + for (int y = 0; y < startingSize.y; y++) { + var layerOffset = y * startingSize.x * startingSize.z; + for (int z = 0; z < startingSize.z; z++) { + var rowOffset = z * startingSize.x; + for (int x = 0; x < startingSize.x; x++) { + var nodeIndexXZ = rowOffset + x; + var node = nodes[layerOffset + nodeIndexXZ]; + if (node != null) node.NodeInGridIndex = nodeIndexXZ; + } + } + } + Profiler.EndSample(); + + Profiler.BeginSample("Clearing custom connections"); + var layers = graph.LayerCount; + for (int i = 0; i < rects.Length; i++) { + var r = rects[i]; + for (int y = 0; y < layers; y++) { + var layerOffset = y * graph.width * graph.depth; + for (int z = r.ymin; z <= r.ymax; z++) { + var rowOffset = z * graph.width + layerOffset; + for (int x = r.xmin; x <= r.xmax; x++) { + var node = nodes[rowOffset + x]; + if (node != null) { + // Clear connections on all nodes that are wrapped and placed on the other side of the graph. + // This is both to clear any custom connections (which do not really make sense after moving the node) + // and to prevent possible exceptions when the node will later (possibly) be destroyed because it was + // not needed anymore (only for layered grid graphs). + node.ClearCustomConnections(true); + } + } + } + } + } + Profiler.EndSample(); + for (int i = 0; i < promises.Length; i++) { + promises[i].Apply(ctx); + } + // Move the center (this is in world units, so we need to convert it back from graph space) + graph.center += graph.transform.TransformVector(new Vector3(dx, 0, dz)); + graph.UpdateTransform(); + + if (promises.Length > 0) graph.rules.ExecuteRuleMainThread(GridGraphRule.Pass.AfterApplied, (promises[0] as GridGraphUpdatePromise).context); + } + } + + class GridGraphUpdatePromise : IGraphUpdatePromise { + /// Reference to a nodes array to allow multiple serial updates to have a common reference to the nodes + public class NodesHolder { + public GridNodeBase[] nodes; + } + public GridGraph graph; + public NodesHolder nodes; + public JobDependencyTracker dependencyTracker; + public int3 nodeArrayBounds; + public IntRect rect; + public JobHandle nodesDependsOn; + public Allocator allocationMethod; + public RecalculationMode recalculationMode; + public GraphUpdateObject graphUpdateObject; + IntBounds writeMaskBounds; + internal GridGraphRules.Context context; + bool emptyUpdate; + IntBounds readBounds; + IntBounds fullRecalculationBounds; + public bool ownsJobDependencyTracker = false; + GraphTransform transform; + + public int CostEstimate => fullRecalculationBounds.volume; + + public GridGraphUpdatePromise(GridGraph graph, GraphTransform transform, NodesHolder nodes, int3 nodeArrayBounds, IntRect rect, JobDependencyTracker dependencyTracker, JobHandle nodesDependsOn, Allocator allocationMethod, RecalculationMode recalculationMode, GraphUpdateObject graphUpdateObject, bool ownsJobDependencyTracker) { + this.graph = graph; + this.transform = transform; + this.nodes = nodes; + this.nodeArrayBounds = nodeArrayBounds; + this.dependencyTracker = dependencyTracker; + this.nodesDependsOn = nodesDependsOn; + this.allocationMethod = allocationMethod; + this.recalculationMode = recalculationMode; + this.graphUpdateObject = graphUpdateObject; + this.ownsJobDependencyTracker = ownsJobDependencyTracker; + CalculateRectangles(graph, rect, out this.rect, out var fullRecalculationRect, out var writeMaskRect, out var readRect); + + if (recalculationMode == RecalculationMode.RecalculateFromScratch) { + // If we are not allowed to read from the graph, we need to recalculate everything that we would otherwise just have read from the graph + fullRecalculationRect = readRect; + } + + // Check if there is anything to do. The bounds may not even overlap the graph. + // Note that writeMaskRect may overlap the graph even though fullRecalculationRect is invalid. + // We ignore that case however since any changes we might write can only be caused by a node that is actually recalculated. + if (!fullRecalculationRect.IsValid()) { + emptyUpdate = true; + } + + // Note that IntRects are defined with inclusive (min,max) coordinates while IntBounds use an exclusive upper bounds. + readBounds = new IntBounds(readRect.xmin, 0, readRect.ymin, readRect.xmax + 1, nodeArrayBounds.y, readRect.ymax + 1); + fullRecalculationBounds = new IntBounds(fullRecalculationRect.xmin, 0, fullRecalculationRect.ymin, fullRecalculationRect.xmax + 1, nodeArrayBounds.y, fullRecalculationRect.ymax + 1); + writeMaskBounds = new IntBounds(writeMaskRect.xmin, 0, writeMaskRect.ymin, writeMaskRect.xmax + 1, nodeArrayBounds.y, writeMaskRect.ymax + 1); + + // If recalculating a very small number of nodes, then disable dependency tracking and just run jobs one after the other. + // This is faster since dependency tracking has some overhead + if (ownsJobDependencyTracker) dependencyTracker.SetLinearDependencies(CostEstimate < 500); + } + + /// Calculates the rectangles used for different purposes during a graph update. + /// The graph + /// The rectangle to update. Anything inside this rectangle may have changed (which may affect nodes outside this rectangle as well). + /// The original rectangle passed to the update method, clamped to the grid. + /// The rectangle of nodes which will be recalculated from scratch. + /// The rectangle of nodes which will have their results written back to the graph. + /// The rectangle of nodes which we need to read from in order to recalculate all nodes in writeMaskRect correctly. + public static void CalculateRectangles (GridGraph graph, IntRect rect, out IntRect originalRect, out IntRect fullRecalculationRect, out IntRect writeMaskRect, out IntRect readRect) { + fullRecalculationRect = rect; + var collision = graph.collision; + if (collision.collisionCheck && collision.type != ColliderType.Ray) fullRecalculationRect = fullRecalculationRect.Expand(Mathf.FloorToInt(collision.diameter * 0.5f + 0.5f)); + + // Rectangle of nodes which will have their results written back to the node class objects. + // Due to erosion a bit more of the graph may be affected by the updates in the fullRecalculationBounds. + writeMaskRect = fullRecalculationRect.Expand(graph.erodeIterations + 1); + + // Rectangle of nodes which we need to read from in order to recalculate all nodes in writeMaskRect correctly. + // Due to how erosion works we need to recalculate erosion in an even larger region to make sure we + // get the correct result inside the writeMask + readRect = writeMaskRect.Expand(graph.erodeIterations + 1); + + // Clamp to the grid dimensions + var gridRect = new IntRect(0, 0, graph.width - 1, graph.depth - 1); + readRect = IntRect.Intersection(readRect, gridRect); + fullRecalculationRect = IntRect.Intersection(fullRecalculationRect, gridRect); + writeMaskRect = IntRect.Intersection(writeMaskRect, gridRect); + originalRect = IntRect.Intersection(rect, gridRect); + } + + + public IEnumerator Prepare () { + if (emptyUpdate) yield break; + + var collision = graph.collision; + var rules = graph.rules; + + if (recalculationMode != RecalculationMode.RecalculateFromScratch) { + // In case a previous graph update has changed the number of layers in the graph + writeMaskBounds.max.y = fullRecalculationBounds.max.y = readBounds.max.y = graph.nodeData.bounds.max.y; + } + + // We never reduce the number of layers in an existing graph. + // Unless we are scanning the graph (not doing an update). + var minLayers = recalculationMode == RecalculationMode.RecalculateFromScratch ? 1 : fullRecalculationBounds.max.y; + + if (recalculationMode == RecalculationMode.RecalculateMinimal && readBounds == fullRecalculationBounds) { + // There is no point reading from the graph since we are recalculating all those nodes anyway. + // This happens if an update is done to the whole graph. + // Skipping the read can improve performance quite a lot for that kind of updates. + // This is purely an optimization and should not change the result. + recalculationMode = RecalculationMode.RecalculateFromScratch; + } + +#if ASTAR_DEBUG + var debugMatrix = graph.transform.matrix; + // using (Draw.WithDuration(1)) { + using (Draw.WithLineWidth(2)) { + using (Draw.WithMatrix(debugMatrix)) { + Draw.xz.WireRectangle(Rect.MinMaxRect(fullRecalculationBounds.min.x, fullRecalculationBounds.min.z, fullRecalculationBounds.max.x, fullRecalculationBounds.max.z), Color.yellow); + } + using (Draw.WithMatrix(debugMatrix * Matrix4x4.Translate(Vector3.up*0.1f))) { + Draw.xz.WireRectangle(Rect.MinMaxRect(writeMaskBounds.min.x, writeMaskBounds.min.z, writeMaskBounds.max.x, writeMaskBounds.max.z), Color.magenta); + Draw.xz.WireRectangle(Rect.MinMaxRect(readBounds.min.x, readBounds.min.z, readBounds.max.x, readBounds.max.z), Color.blue); + Draw.xz.WireRectangle((Rect)rect, Color.green); + } + } +#endif + + var layeredDataLayout = graph is LayerGridGraph; + float characterHeight = graph is LayerGridGraph lg ? lg.characterHeight : float.PositiveInfinity; + + context = new GridGraphRules.Context { + graph = graph, + data = new GridGraphScanData { + dependencyTracker = dependencyTracker, + transform = transform, + up = transform.TransformVector(Vector3.up).normalized, + } + }; + + if (recalculationMode == RecalculationMode.RecalculateFromScratch || recalculationMode == RecalculationMode.RecalculateMinimal) { + var heightCheck = collision.heightCheck && !collision.use2D; + if (heightCheck) { + var layerCount = dependencyTracker.NewNativeArray(1, allocationMethod, NativeArrayOptions.UninitializedMemory); + yield return context.data.HeightCheck(collision, graph.MaxLayers, fullRecalculationBounds, layerCount, characterHeight, allocationMethod); + // The size of the buffers depend on the height check for layered grid graphs since the number of layers might change. + // Never reduce the layer count of the graph. + // Unless we are recalculating the whole graph: in that case we don't care about the existing layers. + // For (not layered) grid graphs this is always 1. + var layers = Mathf.Max(minLayers, layerCount[0]); + readBounds.max.y = fullRecalculationBounds.max.y = writeMaskBounds.max.y = layers; + context.data.heightHitsBounds.max.y = layerCount[0]; + context.data.nodes = new GridGraphNodeData { + bounds = fullRecalculationBounds, + numNodes = fullRecalculationBounds.volume, + layeredDataLayout = layeredDataLayout, + allocationMethod = allocationMethod, + }; + context.data.nodes.AllocateBuffers(dependencyTracker); + + // Set the positions to be used if the height check ray didn't hit anything + context.data.SetDefaultNodePositions(transform); + context.data.CopyHits(context.data.heightHitsBounds); + context.data.CalculateWalkabilityFromHeightData(graph.useRaycastNormal, collision.unwalkableWhenNoGround, graph.maxSlope, characterHeight); + } else { + context.data.nodes = new GridGraphNodeData { + bounds = fullRecalculationBounds, + numNodes = fullRecalculationBounds.volume, + layeredDataLayout = layeredDataLayout, + allocationMethod = allocationMethod, + }; + context.data.nodes.AllocateBuffers(dependencyTracker); + context.data.SetDefaultNodePositions(transform); + // Mark all nodes as walkable to begin with + context.data.nodes.walkable.MemSet(true).Schedule(dependencyTracker); + // Set the normals to point straight up + context.data.nodes.normals.MemSet(new float4(context.data.up.x, context.data.up.y, context.data.up.z, 0)).Schedule(dependencyTracker); + } + + context.data.SetDefaultPenalties(graph.initialPenalty); + + // Kick off jobs early while we prepare the rest of them + JobHandle.ScheduleBatchedJobs(); + + rules.RebuildIfNecessary(); + + { + // Here we execute some rules and possibly wait for some dependencies to complete. + // If main thread rules are used then we need to wait for all previous jobs to complete before the rule is actually executed. + var wait = rules.ExecuteRule(GridGraphRule.Pass.BeforeCollision, context); + while (wait.MoveNext()) yield return wait.Current; + } + + if (collision.collisionCheck) { + context.tracker.timeSlice = TimeSlice.MillisFromNow(1); + var wait = context.data.CollisionCheck(collision, fullRecalculationBounds); + while (wait != null && wait.MoveNext()) { + yield return wait.Current; + context.tracker.timeSlice = TimeSlice.MillisFromNow(2); + } + } + + { + var wait = rules.ExecuteRule(GridGraphRule.Pass.BeforeConnections, context); + while (wait.MoveNext()) yield return wait.Current; + } + + if (recalculationMode == RecalculationMode.RecalculateMinimal) { + // context.data.nodes = context.data.nodes.ReadFromNodesAndCopy(nodes, new Slice3D(nodeArrayBounds, readBounds), nodesDependsOn, graph.nodeData.normals, graphUpdateObject != null ? graphUpdateObject.resetPenaltyOnPhysics : true, dependencyTracker); + var newNodes = new GridGraphNodeData { + bounds = readBounds, + numNodes = readBounds.volume, + layeredDataLayout = layeredDataLayout, + allocationMethod = allocationMethod, + }; + newNodes.AllocateBuffers(dependencyTracker); + // If our layer count is increased, then some nodes may end up with uninitialized normals if we didn't do this memset + newNodes.normals.MemSet(float4.zero).Schedule(dependencyTracker); + newNodes.walkable.MemSet(false).Schedule(dependencyTracker); + newNodes.walkableWithErosion.MemSet(false).Schedule(dependencyTracker); + newNodes.CopyFrom(graph.nodeData, true, dependencyTracker); + newNodes.CopyFrom(context.data.nodes, graphUpdateObject != null ? graphUpdateObject.resetPenaltyOnPhysics : true, dependencyTracker); + context.data.nodes = newNodes; + } + } else { + // If we are not allowed to recalculate the graph then we read all the necessary info from the existing nodes + // context.data.nodes = GridGraphNodeData.ReadFromNodes(nodes, new Slice3D(nodeArrayBounds, readBounds), nodesDependsOn, graph.nodeData.normals, allocationMethod, context.data.nodes.layeredDataLayout, dependencyTracker); + + context.data.nodes = new GridGraphNodeData { + bounds = readBounds, + numNodes = readBounds.volume, + layeredDataLayout = layeredDataLayout, + allocationMethod = allocationMethod, + }; + UnityEngine.Assertions.Assert.IsTrue(graph.nodeData.bounds.Contains(context.data.nodes.bounds)); + context.data.nodes.AllocateBuffers(dependencyTracker); + context.data.nodes.CopyFrom(graph.nodeData, true, dependencyTracker); + } + + if (graphUpdateObject != null) { + // The GraphUpdateObject has an empty implementation of WillUpdateNode, + // so we only need to call it if we are dealing with a subclass of GraphUpdateObject. + // The WillUpdateNode method will be deprecated in the future. + if (graphUpdateObject.GetType() != typeof(GraphUpdateObject)) { + // Mark nodes that might be changed + var nodes = this.nodes.nodes; + for (int y = writeMaskBounds.min.y; y < writeMaskBounds.max.y; y++) { + for (int z = writeMaskBounds.min.z; z < writeMaskBounds.max.z; z++) { + var rowOffset = y*nodeArrayBounds.x*nodeArrayBounds.z + z*nodeArrayBounds.x; + for (int x = writeMaskBounds.min.x; x < writeMaskBounds.max.x; x++) { + graphUpdateObject.WillUpdateNode(nodes[rowOffset + x]); + } + } + } + } + + var updateRect = rect; + if (updateRect.IsValid()) { + // Note that IntRects are defined with inclusive (min,max) coordinates while IntBounds use exclusive upper bounds. + var updateBounds = new IntBounds(updateRect.xmin, 0, updateRect.ymin, updateRect.xmax + 1, context.data.nodes.layers, updateRect.ymax + 1).Offset(-context.data.nodes.bounds.min); + var nodeIndices = dependencyTracker.NewNativeArray(updateBounds.volume, context.data.nodes.allocationMethod, NativeArrayOptions.ClearMemory); + int i = 0; + var dataBoundsSize = context.data.nodes.bounds.size; + for (int y = updateBounds.min.y; y < updateBounds.max.y; y++) { + for (int z = updateBounds.min.z; z < updateBounds.max.z; z++) { + var rowOffset = y*dataBoundsSize.x*dataBoundsSize.z + z*dataBoundsSize.x; + for (int x = updateBounds.min.x; x < updateBounds.max.x; x++) { + nodeIndices[i++] = rowOffset + x; + } + } + } + graphUpdateObject.ApplyJob(new GraphUpdateObject.GraphUpdateData { + nodePositions = context.data.nodes.positions, + nodePenalties = context.data.nodes.penalties, + nodeWalkable = context.data.nodes.walkable, + nodeTags = context.data.nodes.tags, + nodeIndices = nodeIndices, + }, dependencyTracker); + } + } + + // Calculate the connections between nodes and also erode the graph + context.data.Connections(graph.maxStepHeight, graph.maxStepUsesSlope, context.data.nodes.bounds, graph.neighbours, graph.cutCorners, collision.use2D, false, characterHeight); + { + var wait = rules.ExecuteRule(GridGraphRule.Pass.AfterConnections, context); + while (wait.MoveNext()) yield return wait.Current; + } + + if (graph.erodeIterations > 0) { + context.data.Erosion(graph.neighbours, graph.erodeIterations, writeMaskBounds, graph.erosionUseTags, graph.erosionFirstTag, graph.erosionTagsPrecedenceMask); + { + var wait = rules.ExecuteRule(GridGraphRule.Pass.AfterErosion, context); + while (wait.MoveNext()) yield return wait.Current; + } + + // After erosion is done we need to recalculate the node connections + context.data.Connections(graph.maxStepHeight, graph.maxStepUsesSlope, context.data.nodes.bounds, graph.neighbours, graph.cutCorners, collision.use2D, true, characterHeight); + { + var wait = rules.ExecuteRule(GridGraphRule.Pass.AfterConnections, context); + while (wait.MoveNext()) yield return wait.Current; + } + } else { + // If erosion is disabled we can just copy nodeWalkable to nodeWalkableWithErosion + // TODO: Can we just do an assignment of the whole array? + context.data.nodes.walkable.CopyToJob(context.data.nodes.walkableWithErosion).Schedule(dependencyTracker); + } + + { + var wait = rules.ExecuteRule(GridGraphRule.Pass.PostProcess, context); + while (wait.MoveNext()) yield return wait.Current; + } + + // Make the graph's buffers be tracked by the dependency tracker, + // so that they can be disposed automatically, unless we persist them. + graph.nodeData.TrackBuffers(dependencyTracker); + + if (recalculationMode == RecalculationMode.RecalculateFromScratch) { + UnityEngine.Assertions.Assert.AreEqual(Allocator.Persistent, context.data.nodes.allocationMethod); + graph.nodeData = context.data.nodes; + } else { + // Copy node data back to the graph's buffer + graph.nodeData.ResizeLayerCount(context.data.nodes.layers, dependencyTracker); + graph.nodeData.CopyFrom(context.data.nodes, writeMaskBounds, true, dependencyTracker); + } + + graph.nodeData.PersistBuffers(dependencyTracker); + + // We need to wait for the nodes array to be fully initialized before trying to resize it or reading from it + yield return nodesDependsOn; + + yield return dependencyTracker.AllWritesDependency; + + dependencyTracker.ClearMemory(); + } + + public void Apply (IGraphUpdateContext ctx) { + graph.AssertSafeToUpdateGraph(); + if (emptyUpdate) { + Dispose(); + return; + } + + var destroyPreviousNodes = nodes.nodes != graph.nodes; + // For layered grid graphs, we may need to allocate more nodes for the upper layers + if (context.data.nodes.layers > 1) { + nodeArrayBounds.y = context.data.nodes.layers; + var newNodeCount = nodeArrayBounds.x*nodeArrayBounds.y*nodeArrayBounds.z; + // Resize the nodes array. + // We reference it via a shared reference, so that if any other updates will run after this one, + // they will see the resized nodes array immediately. + Memory.Realloc(ref nodes.nodes, newNodeCount); + + // This job needs to be executed on the main thread + // TODO: Do we need writeMaskBounds to prevent allocating nodes outside the permitted region? + new JobAllocateNodes { + active = graph.active, + nodeNormals = graph.nodeData.normals, + dataBounds = context.data.nodes.bounds, + nodeArrayBounds = nodeArrayBounds, + nodes = nodes.nodes, + newGridNodeDelegate = graph.newGridNodeDelegate, + }.Execute(); + } + + var assignToNodesJob = graph.nodeData.AssignToNodes(this.nodes.nodes, nodeArrayBounds, writeMaskBounds, graph.graphIndex, default, dependencyTracker); + assignToNodesJob.Complete(); + + // Destroy the old nodes (if any) and assign the new nodes as an atomic operation from the main thread's perspective + if (nodes.nodes != graph.nodes) { + if (destroyPreviousNodes) { + graph.DestroyAllNodes(); + } + graph.nodes = nodes.nodes; + graph.LayerCount = context.data.nodes.layers; + } + + // Recalculate off mesh links in the affected area + ctx.DirtyBounds(graph.GetBoundsFromRect(new IntRect(writeMaskBounds.min.x, writeMaskBounds.min.z, writeMaskBounds.max.x - 1, writeMaskBounds.max.z - 1))); + Dispose(); + } + + public void Dispose () { + if (ownsJobDependencyTracker) { + ObjectPool.Release(ref dependencyTracker); + if (context != null) context.data.dependencyTracker = null; + } + } + } + + protected override IGraphUpdatePromise ScanInternal (bool async) { + if (nodeSize <= 0) { + return null; + } + + // Make sure the matrix is up to date + UpdateTransform(); + +#if !ASTAR_LARGER_GRIDS + if (width > 1024 || depth > 1024) { + Debug.LogError("One of the grid's sides is longer than 1024 nodes"); + return null; + } +#endif + + SetUpOffsetsAndCosts(); + + // Set a global reference to this graph so that nodes can find it + GridNode.SetGridGraph((int)graphIndex, this); + + // Create and initialize the collision class + collision ??= new GraphCollision(); + collision.Initialize(transform, nodeSize); + + + // Used to allocate buffers for jobs + var dependencyTracker = ObjectPool.Claim(); + + // Create all nodes + var newNodes = AllocateNodesJob(width * depth, out var allocateNodesJob); + + // TODO: Set time slicing in dependency tracker + return new GridGraphUpdatePromise( + graph: this, + transform: transform, + nodes: new GridGraphUpdatePromise.NodesHolder { nodes = newNodes }, + nodeArrayBounds: new int3(width, 1, depth), + rect: new IntRect(0, 0, width - 1, depth - 1), + dependencyTracker: dependencyTracker, + nodesDependsOn: allocateNodesJob, + allocationMethod: Allocator.Persistent, + recalculationMode: RecalculationMode.RecalculateFromScratch, + graphUpdateObject: null, + ownsJobDependencyTracker: true + ); + } + + /// + /// Set walkability for multiple nodes at once. + /// + /// If you are calculating your graph's walkability in some custom way, you can use this method to copy that data to the graph. + /// In most cases you'll not use this method, but instead build your world with colliders and such, and then scan the graph. + /// + /// Note: Any other graph updates may overwrite this data. + /// + /// + /// AstarPath.active.AddWorkItem(() => { + /// var grid = AstarPath.active.data.gridGraph; + /// // Mark all nodes in a 10x10 square, in the top-left corner of the graph, as unwalkable. + /// grid.SetWalkability(new bool[10*10], new IntRect(0, 0, 9, 9)); + /// }); + /// + /// + /// See: grid-rules (view in online documentation for working links) for an alternative way of modifying the graph's walkability. It is more flexible and robust, but requires a bit more code. + /// + public void SetWalkability (bool[] walkability, IntRect rect) { + AssertSafeToUpdateGraph(); + var gridRect = new IntRect(0, 0, width - 1, depth - 1); + if (!gridRect.Contains(rect)) throw new System.ArgumentException("Rect (" + rect + ") must be within the graph bounds (" + gridRect + ")"); + if (walkability.Length != rect.Width*rect.Height) throw new System.ArgumentException("Array must have the same length as rect.Width*rect.Height"); + if (LayerCount != 1) throw new System.InvalidOperationException("This method only works in single-layered grid graphs."); + + for (int z = 0; z < rect.Height; z++) { + var offset = (z + rect.ymin) * width + rect.xmin; + for (int x = 0; x < rect.Width; x++) { + var w = walkability[z * rect.Width + x]; + nodes[offset + x].WalkableErosion = w; + nodes[offset + x].Walkable = w; + } + } + + // Recalculate connections for all affected nodes and their neighbours + RecalculateConnectionsInRegion(rect.Expand(1)); + } + + /// + /// Recalculates node connections for all nodes in grid graph. + /// + /// This is used if you have manually changed the walkability, or other parameters, of some grid nodes, and you need their connections to be recalculated. + /// If you are changing the connections themselves, you should use the and functions instead. + /// + /// Typically you do not change walkability manually. Instead you can use for example a . + /// + /// Note: This will not take into account any grid graph rules that modify connections. So if you have any of those added to the grid graph, you probably want to do a regular graph update instead. + /// + /// See: graph-updates (view in online documentation for working links) + /// See: + /// See: + /// + public void RecalculateAllConnections () { + RecalculateConnectionsInRegion(new IntRect(0, 0, width - 1, depth - 1)); + } + + /// + /// Recalculates node connections for all nodes in a given region of the grid. + /// + /// This is used if you have manually changed the walkability, or other parameters, of some grid nodes, and you need their connections to be recalculated. + /// If you are changing the connections themselves, you should use the and functions instead. + /// + /// Typically you do not change walkability manually. Instead you can use for example a . + /// + /// Warning: This method has some constant overhead, so if you are making several changes to the graph, it is best to batch these updates and only make a single call to this method. + /// + /// Note: This will not take into account any grid graph rules that modify connections. So if you have any of those added to the grid graph, you probably want to do a regular graph update instead. + /// + /// See: graph-updates (view in online documentation for working links) + /// See: + /// See: + /// + public void RecalculateConnectionsInRegion (IntRect recalculateRect) { + AssertSafeToUpdateGraph(); + if (nodes == null || nodes.Length != width * depth * LayerCount) { + throw new System.InvalidOperationException("The Grid Graph is not scanned, cannot recalculate connections."); + } + Assert.AreEqual(new int3(width, LayerCount, depth), nodeData.bounds.size); + + var gridRect = new IntRect(0, 0, width - 1, depth - 1); + var writeRect = IntRect.Intersection(recalculateRect, gridRect); + + // Skip recalculation if the rectangle is outside the graph + if (!writeRect.IsValid()) return; + + var dependencyTracker = ObjectPool.Claim(); + // We need to read node data from the rectangle, and a 1 node border around it in order to be able to calculate connections + // inside the rectangle properly. + var readRect = IntRect.Intersection(writeRect.Expand(1), gridRect); + var readBounds = new IntBounds(readRect.xmin, 0, readRect.ymin, readRect.xmax + 1, LayerCount, readRect.ymax + 1); + if (readBounds.volume < 200) dependencyTracker.SetLinearDependencies(true); + + var layeredDataLayout = this is LayerGridGraph; + var data = new GridGraphScanData { + dependencyTracker = dependencyTracker, + // We can use the temp allocator here because everything will be done before this method returns. + // Unity will normally not let us use these allocations in jobs (presumably because it cannot guarantee that the job will complete before the end of the frame), + // but we will trick it using the UnsafeSpan struct. This is safe because we know that the job will complete before this method returns. + nodes = GridGraphNodeData.ReadFromNodes(nodes, new Slice3D(nodeData.bounds, readBounds), default, nodeData.normals, Allocator.TempJob, layeredDataLayout, dependencyTracker), + transform = transform, + up = transform.WorldUpAtGraphPosition(Vector3.zero), + }; + float characterHeight = this is LayerGridGraph lg ? lg.characterHeight : float.PositiveInfinity; + + var writeBounds = new IntBounds(writeRect.xmin, 0, writeRect.ymin, writeRect.xmax + 1, LayerCount, writeRect.ymax + 1); + data.Connections(maxStepHeight, maxStepUsesSlope, writeBounds, neighbours, cutCorners, collision.use2D, true, characterHeight); + this.nodeData.CopyFrom(data.nodes, writeBounds, true, dependencyTracker); + dependencyTracker.AllWritesDependency.Complete(); + Profiler.BeginSample("Write connections"); + data.AssignNodeConnections(nodes, new int3(width, LayerCount, depth), writeBounds); + Profiler.EndSample(); + ObjectPool.Release(ref dependencyTracker); + + // Recalculate off mesh links in the affected area + active.DirtyBounds(GetBoundsFromRect(writeRect)); + } + + /// + /// Calculates the grid connections for a cell as well as its neighbours. + /// This is a useful utility function if you want to modify the walkability of a single node in the graph. + /// + /// + /// AstarPath.active.AddWorkItem(ctx => { + /// var grid = AstarPath.active.data.gridGraph; + /// int x = 5; + /// int z = 7; + /// + /// // Mark a single node as unwalkable + /// grid.GetNode(x, z).Walkable = false; + /// + /// // Recalculate the connections for that node as well as its neighbours + /// grid.CalculateConnectionsForCellAndNeighbours(x, z); + /// }); + /// + /// + /// Warning: If you are recalculating connections for a lot of nodes at the same time, use instead, since that will be much faster. + /// + public void CalculateConnectionsForCellAndNeighbours (int x, int z) { + RecalculateConnectionsInRegion(new IntRect(x - 1, z - 1, x + 1, z + 1)); + } + + /// + /// Calculates the grid connections for a single node. + /// Convenience function, it's slightly faster to use CalculateConnections(int,int) + /// but that will only show when calculating for a large number of nodes. + /// This function will also work for both grid graphs and layered grid graphs. + /// + /// Deprecated: This method is very slow since 4.3.80. Use or instead to batch connection recalculations. + /// + [System.Obsolete("This method is very slow since 4.3.80. Use RecalculateConnectionsInRegion or RecalculateAllConnections instead to batch connection recalculations.")] + public virtual void CalculateConnections (GridNodeBase node) { + int index = node.NodeInGridIndex; + int x = index % width; + int z = index / width; + + CalculateConnections(x, z); + } + + /// + /// Calculates the grid connections for a single node. + /// Note that to ensure that connections are completely up to date after updating a node you + /// have to calculate the connections for both the changed node and its neighbours. + /// + /// In a layered grid graph, this will recalculate the connections for all nodes + /// in the (x,z) cell (it may have multiple layers of nodes). + /// + /// See: CalculateConnections(GridNodeBase) + /// + /// Deprecated: This method is very slow since 4.3.80. Use instead to batch connection recalculations. + /// + [System.Obsolete("This method is very slow since 4.3.80. Use RecalculateConnectionsInRegion instead to batch connection recalculations.")] + public virtual void CalculateConnections (int x, int z) { + RecalculateConnectionsInRegion(new IntRect(x, z, x, z)); + } + + public override void OnDrawGizmos (DrawingData gizmos, bool drawNodes, RedrawScope redrawScope) { + using (var helper = GraphGizmoHelper.GetSingleFrameGizmoHelper(gizmos, active, redrawScope)) { + // The width and depth fields might not be up to date, so recalculate + // them from the #unclampedSize field + int w, d; + float s; + CalculateDimensions(out w, out d, out s); + var bounds = new Bounds(); + bounds.SetMinMax(Vector3.zero, new Vector3(w, 0, d)); + using (helper.builder.WithMatrix(CalculateTransform().matrix)) { + helper.builder.WireBox(bounds, Color.white); + + int nodeCount = nodes != null ? nodes.Length : -1; + + if (drawNodes && width*depth*LayerCount != nodeCount) { + var color = new Color(1, 1, 1, 0.2f); + helper.builder.WireGrid(new float3(w*0.5f, 0, d*0.5f), Quaternion.identity, new int2(w, d), new float2(w, d), color); + } + } + } + + if (!drawNodes) { + return; + } + + // Loop through chunks of size chunkWidth*chunkWidth and create a gizmo mesh for each of those chunks. + // This is done because rebuilding the gizmo mesh (such as when using Unity Gizmos) every frame is pretty slow + // for large graphs. However just checking if any mesh needs to be updated is relatively fast. So we just store + // a hash together with the mesh and rebuild the mesh when necessary. + const int chunkWidth = 32; + GridNodeBase[] allNodes = ArrayPool.Claim(chunkWidth*chunkWidth*LayerCount); + for (int cx = width/chunkWidth; cx >= 0; cx--) { + for (int cz = depth/chunkWidth; cz >= 0; cz--) { + Profiler.BeginSample("Hash"); + var allNodesCount = GetNodesInRegion(new IntRect(cx*chunkWidth, cz*chunkWidth, (cx+1)*chunkWidth - 1, (cz+1)*chunkWidth - 1), allNodes); + var hasher = new NodeHasher(active); + hasher.Add(showMeshOutline); + hasher.Add(showMeshSurface); + hasher.Add(showNodeConnections); + for (int i = 0; i < allNodesCount; i++) { + hasher.HashNode(allNodes[i]); + } + Profiler.EndSample(); + + if (!gizmos.Draw(hasher, redrawScope)) { + Profiler.BeginSample("Rebuild Retained Gizmo Chunk"); + using (var helper = GraphGizmoHelper.GetGizmoHelper(gizmos, active, hasher, redrawScope)) { + if (showNodeConnections) { + for (int i = 0; i < allNodesCount; i++) { + // Don't bother drawing unwalkable nodes + if (allNodes[i].Walkable) { + helper.DrawConnections(allNodes[i]); + } + } + } + if (showMeshSurface || showMeshOutline) CreateNavmeshSurfaceVisualization(allNodes, allNodesCount, helper); + } + Profiler.EndSample(); + } + } + } + ArrayPool.Release(ref allNodes); + + if (active.showUnwalkableNodes) DrawUnwalkableNodes(gizmos, nodeSize * 0.3f, redrawScope); + } + + /// + /// Draw the surface as well as an outline of the grid graph. + /// The nodes will be drawn as squares (or hexagons when using = Six). + /// + void CreateNavmeshSurfaceVisualization (GridNodeBase[] nodes, int nodeCount, GraphGizmoHelper helper) { + // Count the number of nodes that we will render + int walkable = 0; + + for (int i = 0; i < nodeCount; i++) { + if (nodes[i].Walkable) walkable++; + } + + var neighbourIndices = neighbours == NumNeighbours.Six ? hexagonNeighbourIndices : new [] { 0, 1, 2, 3 }; + var offsetMultiplier = neighbours == NumNeighbours.Six ? 0.333333f : 0.5f; + + // 2 for a square-ish grid, 4 for a hexagonal grid. + var trianglesPerNode = neighbourIndices.Length-2; + var verticesPerNode = 3*trianglesPerNode; + + // Get arrays that have room for all vertices/colors (the array might be larger) + var vertices = ArrayPool.Claim(walkable*verticesPerNode); + var colors = ArrayPool.Claim(walkable*verticesPerNode); + int baseIndex = 0; + + for (int i = 0; i < nodeCount; i++) { + var node = nodes[i]; + if (!node.Walkable) continue; + + var nodeColor = helper.NodeColor(node); + // Don't bother drawing transparent nodes + if (nodeColor.a <= 0.001f) continue; + + for (int dIndex = 0; dIndex < neighbourIndices.Length; dIndex++) { + // For neighbours != Six + // n2 -- n3 + // | | + // n -- n1 + // + // n = this node + var d = neighbourIndices[dIndex]; + var nextD = neighbourIndices[(dIndex + 1) % neighbourIndices.Length]; + GridNodeBase n1, n2, n3 = null; + n1 = node.GetNeighbourAlongDirection(d); + if (n1 != null && neighbours != NumNeighbours.Six) { + n3 = n1.GetNeighbourAlongDirection(nextD); + } + + n2 = node.GetNeighbourAlongDirection(nextD); + if (n2 != null && n3 == null && neighbours != NumNeighbours.Six) { + n3 = n2.GetNeighbourAlongDirection(d); + } + + // Position in graph space of the vertex + Vector3 p = new Vector3(node.XCoordinateInGrid + 0.5f, 0, node.ZCoordinateInGrid + 0.5f); + // Offset along diagonal to get the correct XZ coordinates + p.x += (neighbourXOffsets[d] + neighbourXOffsets[nextD]) * offsetMultiplier; + p.z += (neighbourZOffsets[d] + neighbourZOffsets[nextD]) * offsetMultiplier; + + // Interpolate the y coordinate of the vertex so that the mesh will be seamless (except in some very rare edge cases) + p.y += transform.InverseTransform((Vector3)node.position).y; + if (n1 != null) p.y += transform.InverseTransform((Vector3)n1.position).y; + if (n2 != null) p.y += transform.InverseTransform((Vector3)n2.position).y; + if (n3 != null) p.y += transform.InverseTransform((Vector3)n3.position).y; + p.y /= (1f + (n1 != null ? 1f : 0f) + (n2 != null ? 1f : 0f) + (n3 != null ? 1f : 0f)); + + // Convert the point from graph space to world space + // This handles things like rotations, scale other transformations + p = transform.Transform(p); + vertices[baseIndex + dIndex] = p; + } + + if (neighbours == NumNeighbours.Six) { + // Form the two middle triangles + vertices[baseIndex + 6] = vertices[baseIndex + 0]; + vertices[baseIndex + 7] = vertices[baseIndex + 2]; + vertices[baseIndex + 8] = vertices[baseIndex + 3]; + + vertices[baseIndex + 9] = vertices[baseIndex + 0]; + vertices[baseIndex + 10] = vertices[baseIndex + 3]; + vertices[baseIndex + 11] = vertices[baseIndex + 5]; + } else { + // Form the last triangle + vertices[baseIndex + 4] = vertices[baseIndex + 0]; + vertices[baseIndex + 5] = vertices[baseIndex + 2]; + } + + // Set all colors for the node + for (int j = 0; j < verticesPerNode; j++) { + colors[baseIndex + j] = nodeColor; + } + + // Draw the outline of the node + for (int j = 0; j < neighbourIndices.Length; j++) { + var other = node.GetNeighbourAlongDirection(neighbourIndices[(j+1) % neighbourIndices.Length]); + // Just a tie breaker to make sure we don't draw the line twice. + // Using NodeInGridIndex instead of NodeIndex to make the gizmos deterministic for a given grid layout. + // This is important because if the graph would be re-scanned and only a small part of it would change + // then most chunks would be cached by the gizmo system, but the node indices may have changed and + // if NodeIndex was used then we might get incorrect gizmos at the borders between chunks. + if (other == null || (showMeshOutline && node.NodeInGridIndex < other.NodeInGridIndex)) { + helper.builder.Line(vertices[baseIndex + j], vertices[baseIndex + (j+1) % neighbourIndices.Length], other == null ? Color.black : nodeColor); + } + } + + baseIndex += verticesPerNode; + } + + if (showMeshSurface) helper.DrawTriangles(vertices, colors, baseIndex*trianglesPerNode/verticesPerNode); + + ArrayPool.Release(ref vertices); + ArrayPool.Release(ref colors); + } + + /// + /// Bounding box in world space which encapsulates all nodes in the given rectangle. + /// + /// The bounding box will cover all nodes' surfaces completely. Not just their centers. + /// + /// Note: The bounding box may not be particularly tight if the graph is not axis-aligned. + /// + /// See: + /// + /// Which nodes to consider. Will be clamped to the grid's bounds. If the rectangle is outside the graph, an empty bounds will be returned. + public Bounds GetBoundsFromRect (IntRect rect) { + rect = IntRect.Intersection(rect, new IntRect(0, 0, width-1, depth-1)); + if (!rect.IsValid()) return new Bounds(); + return transform.Transform(new Bounds( + new Vector3(rect.xmin + rect.xmax, collision.fromHeight, rect.ymin + rect.ymax) * 0.5f, + // Note: We add +1 to the width and height to make the bounding box cover the nodes' surfaces completely, instead + // of just their centers. + new Vector3(rect.Width + 1, collision.fromHeight, rect.Height + 1) + )); + } + + /// + /// A rect that contains all nodes that the bounds could touch. + /// This correctly handles rotated graphs and other transformations. + /// The returned rect is guaranteed to not extend outside the graph bounds. + /// + /// Note: The rect may contain nodes that are not contained in the bounding box since the bounding box is aligned to the world, and the rect is aligned to the grid (which may be rotated). + /// + /// See: + /// See: + /// + public IntRect GetRectFromBounds (Bounds bounds) { + // Take the bounds and transform it using the matrix + // Then convert that to a rectangle which contains + // all nodes that might be inside the bounds + + bounds = transform.InverseTransform(bounds); + Vector3 min = bounds.min; + Vector3 max = bounds.max; + + // Allow the bounds to extend a tiny amount into adjacent nodes. + // This is mostly to avoid requiring a much larger update region if a user + // passes a bounding box exactly (plus/minus floating point errors) covering + // a set of nodes. + const float MARGIN = 0.01f; + + int minX = Mathf.FloorToInt(min.x+MARGIN); + int maxX = Mathf.FloorToInt(max.x-MARGIN); + + int minZ = Mathf.FloorToInt(min.z+MARGIN); + int maxZ = Mathf.FloorToInt(max.z-MARGIN); + + var originalRect = new IntRect(minX, minZ, maxX, maxZ); + + // Rect which covers the whole grid + var gridRect = new IntRect(0, 0, width-1, depth-1); + + // Clamp the rect to the grid + return IntRect.Intersection(originalRect, gridRect); + } + + /// + /// All nodes inside the bounding box. + /// Note: Be nice to the garbage collector and pool the list when you are done with it (optional) + /// See: Pathfinding.Util.ListPool + /// + /// See: GetNodesInRegion(GraphUpdateShape) + /// + public List GetNodesInRegion (Bounds bounds) { + return GetNodesInRegion(bounds, null); + } + + /// + /// All nodes inside the shape. + /// Note: Be nice to the garbage collector and pool the list when you are done with it (optional) + /// See: Pathfinding.Util.ListPool + /// + /// See: GetNodesInRegion(Bounds) + /// + public List GetNodesInRegion (GraphUpdateShape shape) { + return GetNodesInRegion(shape.GetBounds(), shape); + } + + /// + /// All nodes inside the shape or if null, the bounding box. + /// If a shape is supplied, it is assumed to be contained inside the bounding box. + /// See: GraphUpdateShape.GetBounds + /// + protected virtual List GetNodesInRegion (Bounds bounds, GraphUpdateShape shape) { + var rect = GetRectFromBounds(bounds); + + if (nodes == null || !rect.IsValid() || nodes.Length != width*depth*LayerCount) { + return ListPool.Claim(); + } + + // Get a buffer we can use + var inArea = ListPool.Claim(rect.Width*rect.Height); + var rw = rect.Width; + + // Loop through all nodes in the rectangle + for (int y = 0; y < LayerCount; y++) { + for (int z = rect.ymin; z <= rect.ymax; z++) { + var offset = y*width*depth + z*width + rect.xmin; + for (int x = 0; x < rw; x++) { + var node = nodes[offset + x]; + if (node == null) continue; + + // If it is contained in the bounds (and optionally the shape) + // then add it to the buffer + var pos = (Vector3)node.position; + if (bounds.Contains(pos) && (shape == null || shape.Contains(pos))) { + inArea.Add(node); + } + } + } + } + + return inArea; + } + + /// + /// Get all nodes in a rectangle. + /// + /// See: + /// + /// Region in which to return nodes. It will be clamped to the grid. + public List GetNodesInRegion (IntRect rect) { + // Clamp the rect to the grid + // Rect which covers the whole grid + var gridRect = new IntRect(0, 0, width-1, depth-1); + + rect = IntRect.Intersection(rect, gridRect); + + if (nodes == null || !rect.IsValid() || nodes.Length != width*depth*LayerCount) return ListPool.Claim(0); + + // Get a buffer we can use + var inArea = ListPool.Claim(rect.Width*rect.Height); + var rw = rect.Width; + + for (int y = 0; y < LayerCount; y++) { + for (int z = rect.ymin; z <= rect.ymax; z++) { + var offset = y*width*depth + z*width + rect.xmin; + for (int x = 0; x < rw; x++) { + var node = nodes[offset + x]; + if (node != null) inArea.Add(node); + } + } + } + + return inArea; + } + + /// + /// Get all nodes in a rectangle. + /// Returns: The number of nodes written to the buffer. + /// + /// Note: This method is much faster than GetNodesInRegion(IntRect) which returns a list because this method can make use of the highly optimized + /// System.Array.Copy method. + /// + /// See: + /// + /// Region in which to return nodes. It will be clamped to the grid. + /// Buffer in which the nodes will be stored. Should be at least as large as the number of nodes that can exist in that region. + public virtual int GetNodesInRegion (IntRect rect, GridNodeBase[] buffer) { + // Clamp the rect to the grid + // Rect which covers the whole grid + var gridRect = new IntRect(0, 0, width-1, depth-1); + + rect = IntRect.Intersection(rect, gridRect); + + if (nodes == null || !rect.IsValid() || nodes.Length != width*depth) return 0; + + if (buffer.Length < rect.Width*rect.Height) throw new System.ArgumentException("Buffer is too small"); + + int counter = 0; + for (int z = rect.ymin; z <= rect.ymax; z++, counter += rect.Width) { + System.Array.Copy(nodes, z*Width + rect.xmin, buffer, counter, rect.Width); + } + + return counter; + } + + /// + /// Node in the specified cell. + /// Returns null if the coordinate is outside the grid. + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// int x = 5; + /// int z = 8; + /// GridNodeBase node = gg.GetNode(x, z); + /// + /// + /// If you know the coordinate is inside the grid and you are looking to maximize performance then you + /// can look up the node in the internal array directly which is slightly faster. + /// See: + /// + public virtual GridNodeBase GetNode (int x, int z) { + if (x < 0 || z < 0 || x >= width || z >= depth) return null; + return nodes[x + z*width]; + } + + class CombinedGridGraphUpdatePromise : IGraphUpdatePromise { + List promises; + + public CombinedGridGraphUpdatePromise(GridGraph graph, List graphUpdates) { + promises = ListPool.Claim(); + var nodesHolder = new GridGraphUpdatePromise.NodesHolder { nodes = graph.nodes }; + + for (int i = 0; i < graphUpdates.Count; i++) { + var graphUpdate = graphUpdates[i]; + var promise = new GridGraphUpdatePromise( + graph: graph, + transform: graph.transform, + nodes: nodesHolder, + nodeArrayBounds: new int3(graph.width, graph.LayerCount, graph.depth), + rect: graph.GetRectFromBounds(graphUpdate.bounds), + dependencyTracker: ObjectPool.Claim(), + nodesDependsOn: default, + allocationMethod: Allocator.Persistent, + recalculationMode: graphUpdate.updatePhysics ? RecalculationMode.RecalculateMinimal : RecalculationMode.NoRecalculation, + graphUpdateObject: graphUpdate, + ownsJobDependencyTracker: true + ); + promises.Add(promise); + } + } + + public IEnumerator Prepare () { + for (int i = 0; i < promises.Count; i++) { + var it = promises[i].Prepare(); + while (it.MoveNext()) yield return it.Current; + } + } + + public void Apply (IGraphUpdateContext ctx) { + for (int i = 0; i < promises.Count; i++) { + promises[i].Apply(ctx); + } + ListPool.Release(ref promises); + } + } + + /// Internal function to update the graph + IGraphUpdatePromise IUpdatableGraph.ScheduleGraphUpdates (List graphUpdates) { + if (!isScanned || nodes.Length != width*depth*LayerCount) { + Debug.LogWarning("The Grid Graph is not scanned, cannot update graph"); + return null; + } + + collision.Initialize(transform, nodeSize); + return new CombinedGridGraphUpdatePromise(this, graphUpdates); + } + + class GridGraphSnapshot : IGraphSnapshot { + internal GridGraphNodeData nodes; + internal GridGraph graph; + + public void Dispose () { + nodes.Dispose(); + } + + public void Restore (IGraphUpdateContext ctx) { + graph.AssertSafeToUpdateGraph(); + if (!graph.isScanned) return; + + if (!graph.nodeData.bounds.Contains(nodes.bounds)) { + Debug.LogError("Cannot restore snapshot because the graph dimensions have changed since the snapshot was taken"); + return; + } + + var dependencyTracker = ObjectPool.Claim(); + graph.nodeData.CopyFrom(nodes, true, dependencyTracker); + + var assignToNodesJob = nodes.AssignToNodes(graph.nodes, graph.nodeData.bounds.size, nodes.bounds, graph.graphIndex, new JobHandle(), dependencyTracker); + assignToNodesJob.Complete(); + dependencyTracker.AllWritesDependency.Complete(); + ObjectPool.Release(ref dependencyTracker); + + // Recalculate off mesh links in the affected area + ctx.DirtyBounds(graph.GetBoundsFromRect(new IntRect(nodes.bounds.min.x, nodes.bounds.min.z, nodes.bounds.max.x - 1, nodes.bounds.max.z - 1))); + } + } + + public override IGraphSnapshot Snapshot (Bounds bounds) { + if (active.isScanning || active.IsAnyWorkItemInProgress) { + throw new System.InvalidOperationException("Trying to capture a grid graph snapshot while inside a work item. This is not supported, as the graphs may be in an inconsistent state."); + } + + if (!isScanned || nodes.Length != width*depth*LayerCount) return null; + + GridGraphUpdatePromise.CalculateRectangles(this, GetRectFromBounds(bounds), out var _, out var _, out var writeMaskRect, out var _); + if (!writeMaskRect.IsValid()) return null; + + var nodeBounds = new IntBounds(writeMaskRect.xmin, 0, writeMaskRect.ymin, writeMaskRect.xmax + 1, LayerCount, writeMaskRect.ymax + 1); + var snapshotData = new GridGraphNodeData { + allocationMethod = Allocator.Persistent, + bounds = nodeBounds, + numNodes = nodeBounds.volume, + }; + snapshotData.AllocateBuffers(null); + snapshotData.CopyFrom(this.nodeData, true, null); + return new GridGraphSnapshot { + nodes = snapshotData, + graph = this, + }; + } + + /// + /// Returns if there is an obstacle between from and to on the graph. + /// This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// bool anyObstaclesInTheWay = gg.Linecast(transform.position, enemy.position); + /// + /// + /// [Open online documentation to see images] + /// + /// Edge cases are handled as follows: + /// - Shared edges and corners between walkable and unwalkable nodes are treated as walkable (so for example if the linecast just touches a corner of an unwalkable node, this is allowed). + /// - If the linecast starts outside the graph, a hit is returned at from. + /// - If the linecast starts inside the graph, but the end is outside of it, a hit is returned at the point where it exits the graph (unless there are any other hits before that). + /// + public bool Linecast (Vector3 from, Vector3 to) { + GraphHitInfo hit; + + return Linecast(from, to, out hit); + } + + /// + /// Returns if there is an obstacle between from and to on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// bool anyObstaclesInTheWay = gg.Linecast(transform.position, enemy.position); + /// + /// + /// [Open online documentation to see images] + /// + /// Deprecated: The hint parameter is deprecated + /// + /// Point to linecast from + /// Point to linecast to + /// This parameter is deprecated. It will be ignored. + [System.Obsolete("The hint parameter is deprecated")] + public bool Linecast (Vector3 from, Vector3 to, GraphNode hint) { + GraphHitInfo hit; + + return Linecast(from, to, hint, out hit); + } + + /// + /// Returns if there is an obstacle between from and to on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// bool anyObstaclesInTheWay = gg.Linecast(transform.position, enemy.position); + /// + /// + /// [Open online documentation to see images] + /// + /// Deprecated: The hint parameter is deprecated + /// + /// Point to linecast from + /// Point to linecast to + /// Contains info on what was hit, see GraphHitInfo + /// This parameter is deprecated. It will be ignored. + [System.Obsolete("The hint parameter is deprecated")] + public bool Linecast (Vector3 from, Vector3 to, GraphNode hint, out GraphHitInfo hit) { + return Linecast(from, to, hint, out hit, null); + } + + /// Magnitude of the cross product a x b + protected static long CrossMagnitude (int2 a, int2 b) { + return (long)a.x*b.y - (long)b.x*a.y; + } + + /// + /// Clips a line segment in graph space to the graph bounds. + /// That is (0,0,0) is the bottom left corner of the graph and (width,0,depth) is the top right corner. + /// The first node is placed at (0.5,y,0.5). One unit distance is the same as nodeSize. + /// + /// Returns false if the line segment does not intersect the graph at all. + /// + protected bool ClipLineSegmentToBounds (Vector3 a, Vector3 b, out Vector3 outA, out Vector3 outB) { + // If the start or end points are outside + // the graph then clamping is needed + if (a.x < 0 || a.z < 0 || a.x > width || a.z > depth || + b.x < 0 || b.z < 0 || b.x > width || b.z > depth) { + // Boundary of the grid + var p1 = new Vector3(0, 0, 0); + var p2 = new Vector3(0, 0, depth); + var p3 = new Vector3(width, 0, depth); + var p4 = new Vector3(width, 0, 0); + + int intersectCount = 0; + + bool intersect; + Vector3 intersection; + + intersection = VectorMath.SegmentIntersectionPointXZ(a, b, p1, p2, out intersect); + + if (intersect) { + intersectCount++; + if (!VectorMath.RightOrColinearXZ(p1, p2, a)) { + a = intersection; + } else { + b = intersection; + } + } + intersection = VectorMath.SegmentIntersectionPointXZ(a, b, p2, p3, out intersect); + + if (intersect) { + intersectCount++; + if (!VectorMath.RightOrColinearXZ(p2, p3, a)) { + a = intersection; + } else { + b = intersection; + } + } + intersection = VectorMath.SegmentIntersectionPointXZ(a, b, p3, p4, out intersect); + + if (intersect) { + intersectCount++; + if (!VectorMath.RightOrColinearXZ(p3, p4, a)) { + a = intersection; + } else { + b = intersection; + } + } + intersection = VectorMath.SegmentIntersectionPointXZ(a, b, p4, p1, out intersect); + + if (intersect) { + intersectCount++; + if (!VectorMath.RightOrColinearXZ(p4, p1, a)) { + a = intersection; + } else { + b = intersection; + } + } + + if (intersectCount == 0) { + // The line does not intersect with the grid + outA = Vector3.zero; + outB = Vector3.zero; + return false; + } + } + + outA = a; + outB = b; + return true; + } + + /// + /// Returns if there is an obstacle between from and to on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// bool anyObstaclesInTheWay = gg.Linecast(transform.position, enemy.position); + /// + /// + /// Deprecated: The hint parameter is deprecated + /// + /// [Open online documentation to see images] + /// + /// Point to linecast from + /// Point to linecast to + /// Contains info on what was hit, see GraphHitInfo + /// This parameter is deprecated. It will be ignored. + /// If a list is passed, then it will be filled with all nodes the linecast traverses + /// If not null then the delegate will be called for each node and if it returns false the node will be treated as unwalkable and a hit will be returned. + /// Note that unwalkable nodes are always treated as unwalkable regardless of what this filter returns. + [System.Obsolete("The hint parameter is deprecated")] + public bool Linecast (Vector3 from, Vector3 to, GraphNode hint, out GraphHitInfo hit, List trace, System.Func filter = null) { + return Linecast(from, to, out hit, trace, filter); + } + + /// + /// Returns if there is an obstacle between from and to on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. + /// + /// Edge cases are handled as follows: + /// - Shared edges and corners between walkable and unwalkable nodes are treated as walkable (so for example if the linecast just touches a corner of an unwalkable node, this is allowed). + /// - If the linecast starts outside the graph, a hit is returned at from. + /// - If the linecast starts inside the graph, but the end is outside of it, a hit is returned at the point where it exits the graph (unless there are any other hits before that). + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// bool anyObstaclesInTheWay = gg.Linecast(transform.position, enemy.position); + /// + /// + /// [Open online documentation to see images] + /// + /// Point to linecast from + /// Point to linecast to + /// Contains info on what was hit, see \reflink{GraphHitInfo}. + /// If a list is passed, then it will be filled with all nodes the linecast traverses + /// If not null then the delegate will be called for each node and if it returns false the node will be treated as unwalkable and a hit will be returned. + /// Note that unwalkable nodes are always treated as unwalkable regardless of what this filter returns. + public bool Linecast (Vector3 from, Vector3 to, out GraphHitInfo hit, List trace = null, System.Func filter = null) { + var res = Linecast(from, to, out GridHitInfo gridHit, trace, filter); + hit = new GraphHitInfo { + origin = from, + node = gridHit.node, + }; + if (res) { + // Hit obstacle + // We know from what direction we moved in + // so we can calculate the line which we hit + var ndir = gridHit.direction; + if (ndir == -1 || gridHit.node == null) { + // We didn't really hit a wall. Possibly the start node was unwalkable or we ended up at the right cell, but wrong floor (layered grid graphs only) + hit.point = gridHit.node == null || !gridHit.node.Walkable || (filter != null && !filter(gridHit.node)) ? from : to; + if (gridHit.node != null) hit.point = gridHit.node.ProjectOnSurface(hit.point); + hit.tangentOrigin = Vector3.zero; + hit.tangent = Vector3.zero; + } else { + Vector3 fromInGraphSpace = transform.InverseTransform(from); + Vector3 toInGraphSpace = transform.InverseTransform(to); + + // Throw away components we don't care about (y) + // Also subtract 0.5 because nodes have an offset of 0.5 (first node is at (0.5,0.5) not at (0,0)) + // And it's just more convenient to remove that term here. + // The variable names #from and #to are unfortunately already taken, so let's use start and end. + var fromInGraphSpace2D = new Vector2(fromInGraphSpace.x - 0.5f, fromInGraphSpace.z - 0.5f); + var toInGraphSpace2D = new Vector2(toInGraphSpace.x - 0.5f, toInGraphSpace.z - 0.5f); + + // Current direction and current direction ±90 degrees + var d1 = new Vector2(neighbourXOffsets[ndir], neighbourZOffsets[ndir]); + var d2 = new Vector2(neighbourXOffsets[(ndir-1+4) & 0x3], neighbourZOffsets[(ndir-1+4) & 0x3]); + Vector2 lineDirection = new Vector2(neighbourXOffsets[(ndir+1) & 0x3], neighbourZOffsets[(ndir+1) & 0x3]); + var p = new Vector2(gridHit.node.XCoordinateInGrid, gridHit.node.ZCoordinateInGrid); + Vector2 lineOrigin = p + (d1 + d2) * 0.5f; + + // Find the intersection + var intersection = VectorMath.LineIntersectionPoint(lineOrigin, lineOrigin+lineDirection, fromInGraphSpace2D, toInGraphSpace2D); + + var currentNodePositionInGraphSpace = transform.InverseTransform((Vector3)gridHit.node.position); + + // The intersection is in graph space (with an offset of 0.5) so we need to transform it to world space + var intersection3D = new Vector3(intersection.x + 0.5f, currentNodePositionInGraphSpace.y, intersection.y + 0.5f); + var lineOrigin3D = new Vector3(lineOrigin.x + 0.5f, currentNodePositionInGraphSpace.y, lineOrigin.y + 0.5f); + + hit.point = transform.Transform(intersection3D); + hit.tangentOrigin = transform.Transform(lineOrigin3D); + hit.tangent = transform.TransformVector(new Vector3(lineDirection.x, 0, lineDirection.y)); + } + } else { + hit.point = to; + } + return res; + } + + /// + /// Returns if there is an obstacle between from and to on the graph. + /// + /// This function is different from the other Linecast functions since it snaps the start and end positions to the centers of the closest nodes on the graph. + /// This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. + /// + /// Version: Since 3.6.8 this method uses the same implementation as the other linecast methods so there is no performance boost to using it. + /// Version: In 3.6.8 this method was rewritten and that fixed a large number of bugs. + /// Previously it had not always followed the line exactly as it should have + /// and the hit output was not very accurate + /// (for example the hit point was just the node position instead of a point on the edge which was hit). + /// + /// Deprecated: Use instead. + /// + /// Point to linecast from. + /// Point to linecast to. + /// Contains info on what was hit, see GraphHitInfo. + /// This parameter is deprecated. It will be ignored. + [System.Obsolete("Use Linecast instead")] + public bool SnappedLinecast (Vector3 from, Vector3 to, GraphNode hint, out GraphHitInfo hit) { + return Linecast( + (Vector3)GetNearest(from, null).node.position, + (Vector3)GetNearest(to, null).node.position, + hint, + out hit + ); + } + + /// + /// Returns if there is an obstacle between the two nodes on the graph. + /// + /// This method is very similar to the other Linecast methods however it is a bit faster + /// due to not having to look up which node is closest to a particular input point. + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// var node1 = gg.GetNode(2, 3); + /// var node2 = gg.GetNode(5, 7); + /// bool anyObstaclesInTheWay = gg.Linecast(node1, node2); + /// + /// + /// Node to start from. + /// Node to try to reach using a straight line. + /// If not null then the delegate will be called for each node and if it returns false the node will be treated as unwalkable and a hit will be returned. + /// Note that unwalkable nodes are always treated as unwalkable regardless of what this filter returns. + public bool Linecast (GridNodeBase fromNode, GridNodeBase toNode, System.Func filter = null) { + var nodeCenter = new int2(FixedPrecisionScale/2, FixedPrecisionScale/2); + return Linecast(fromNode, nodeCenter, toNode, nodeCenter, out GridHitInfo hit, null, filter); + } + + /// + /// Returns if there is an obstacle between from and to on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the graph and looks for collisions. + /// + /// Note: This overload outputs a hit of type instead of . It's a bit faster to calculate this output + /// and it can be useful for some grid-specific algorithms. + /// + /// Edge cases are handled as follows: + /// - Shared edges and corners between walkable and unwalkable nodes are treated as walkable (so for example if the linecast just touches a corner of an unwalkable node, this is allowed). + /// - If the linecast starts outside the graph, a hit is returned at from. + /// - If the linecast starts inside the graph, but the end is outside of it, a hit is returned at the point where it exits the graph (unless there are any other hits before that). + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// bool anyObstaclesInTheWay = gg.Linecast(transform.position, enemy.position); + /// + /// + /// [Open online documentation to see images] + /// + /// Point to linecast from + /// Point to linecast to + /// Contains info on what was hit, see \reflink{GridHitInfo} + /// If a list is passed, then it will be filled with all nodes the linecast traverses + /// If not null then the delegate will be called for each node and if it returns false the node will be treated as unwalkable and a hit will be returned. + /// Note that unwalkable nodes are always treated as unwalkable regardless of what this filter returns. + public bool Linecast (Vector3 from, Vector3 to, out GridHitInfo hit, List trace = null, System.Func filter = null) { + Vector3 fromInGraphSpace = transform.InverseTransform(from); + Vector3 toInGraphSpace = transform.InverseTransform(to); + + // Clip the line so that the start and end points are on the graph + if (!ClipLineSegmentToBounds(fromInGraphSpace, toInGraphSpace, out var fromInGraphSpaceClipped, out var toInGraphSpaceClipped)) { + // Line does not intersect the graph + // So there are no obstacles we can hit + hit = new GridHitInfo { + node = null, + direction = -1, + }; + return false; + } + + // From is outside the graph, but #to is inside. + if ((fromInGraphSpace - fromInGraphSpaceClipped).sqrMagnitude > 0.001f*0.001f) { + hit = new GridHitInfo { + node = null, + direction = -1, + }; + return true; + } + bool toIsOutsideGraph = (toInGraphSpace - toInGraphSpaceClipped).sqrMagnitude > 0.001f*0.001f; + + // Find the closest nodes to the start and end on the part of the segment which is on the graph + var startNode = GetNearestFromGraphSpace(fromInGraphSpaceClipped); + var endNode = GetNearestFromGraphSpace(toInGraphSpaceClipped); + if (startNode == null || endNode == null) { + hit = new GridHitInfo { + node = null, + direction = -1, + }; + return false; + } + + return Linecast( + startNode, new Vector2(fromInGraphSpaceClipped.x - startNode.XCoordinateInGrid, fromInGraphSpaceClipped.z - startNode.ZCoordinateInGrid), + endNode, new Vector2(toInGraphSpaceClipped.x - endNode.XCoordinateInGrid, toInGraphSpaceClipped.z - endNode.ZCoordinateInGrid), + out hit, + trace, + filter, + toIsOutsideGraph + ); + } + + /// + /// Scaling used for the coordinates in the Linecast methods that take normalized points using integer coordinates. + /// + /// To convert from world space, each coordinate is multiplied by this factor and then rounded to the nearest integer. + /// + /// Typically you do not need to use this constant yourself, instead use the Linecast overloads that do not take integer coordinates. + /// + public const int FixedPrecisionScale = 1024; + + /// + /// Returns if there is an obstacle between the two nodes on the graph. + /// + /// This method is very similar to the other Linecast methods but it gives some extra control, in particular when the start/end points are at node corners instead of inside nodes. + /// + /// Shared edges and corners between walkable and unwalkable nodes are treated as walkable. + /// So for example if the linecast just touches a corner of an unwalkable node, this is allowed. + /// + /// Node to start from. + /// Where in the start node to start. This is a normalized value so each component must be in the range 0 to 1 (inclusive). + /// Node to try to reach using a straight line. + /// Where in the end node to end. This is a normalized value so each component must be in the range 0 to 1 (inclusive). + /// Contains info on what was hit, see \reflink{GridHitInfo} + /// If a list is passed, then it will be filled with all nodes the linecast traverses + /// If not null then the delegate will be called for each node and if it returns false the node will be treated as unwalkable and a hit will be returned. + /// Note that unwalkable nodes are always treated as unwalkable regardless of what this filter returns. + /// If true, the linecast will continue past the end point in the same direction until it hits something. + public bool Linecast (GridNodeBase fromNode, Vector2 normalizedFromPoint, GridNodeBase toNode, Vector2 normalizedToPoint, out GridHitInfo hit, List trace = null, System.Func filter = null, bool continuePastEnd = false) { + var fixedNormalizedFromPoint = new int2((int)Mathf.Round(normalizedFromPoint.x*FixedPrecisionScale), (int)Mathf.Round(normalizedFromPoint.y*FixedPrecisionScale)); + var fixedNormalizedToPoint = new int2((int)Mathf.Round(normalizedToPoint.x*FixedPrecisionScale), (int)Mathf.Round(normalizedToPoint.y*FixedPrecisionScale)); + + return Linecast(fromNode, fixedNormalizedFromPoint, toNode, fixedNormalizedToPoint, out hit, trace, filter, continuePastEnd); + } + + /// + /// Returns if there is an obstacle between the two nodes on the graph. + /// Like but takes normalized points as fixed precision points normalized between 0 and FixedPrecisionScale instead of between 0 and 1. + /// + public bool Linecast (GridNodeBase fromNode, int2 fixedNormalizedFromPoint, GridNodeBase toNode, int2 fixedNormalizedToPoint, out GridHitInfo hit, List trace = null, System.Func filter = null, bool continuePastEnd = false) { + /* + * Briefly, the algorithm used in this function can be described as: + * 1. Determine the two axis aligned directions which will bring us closer to the target. + * 2. In each step, check which direction out of those two that the linecast exits the current node from. + * 3. Try to move in that direction if possible. If the linecast exits the current node through a corner, then moving along either direction is allowed. + * 4. If that's not possible, and the line exits the current node at a corner, then try to move to the other side of line to the other row/column. + * 5. If we still couldn't move anywhere, report a hit. + * 6. Go back to step 2. + * + * Sadly the implementation is complicated by numerous edge cases, while trying to keep everything highly performant. + * I've tried to document them as best I could. + * + * TODO: Maybe this could be rewritten such that instead of only being positioned at one node at a time, + * we could be inside up to two nodes at the same time (which share either an edge or a corner). + * This divergence would be done when the linecast line goes through a corner or right in the middle between two nodes. + * This could potentially remove a bunch of edge cases. + */ + if (fixedNormalizedFromPoint.x < 0 || fixedNormalizedFromPoint.x > FixedPrecisionScale) throw new System.ArgumentOutOfRangeException(nameof(fixedNormalizedFromPoint), "must be between 0 and 1024"); + if (fixedNormalizedToPoint.x < 0 || fixedNormalizedToPoint.x > FixedPrecisionScale) throw new System.ArgumentOutOfRangeException(nameof(fixedNormalizedToPoint), "must be between 0 and 1024"); + + if (fromNode == null) throw new System.ArgumentNullException(nameof(fromNode)); + if (toNode == null) throw new System.ArgumentNullException(nameof(toNode)); + + // Use the filter + if ((filter != null && !filter(fromNode)) || !fromNode.Walkable) { + hit = new GridHitInfo { + node = fromNode, + direction = -1, + }; + return true; + } + + if (fromNode == toNode) { + // Fast path, we don't have to do anything + hit = new GridHitInfo { + node = fromNode, + direction = -1, + }; + if (trace != null) trace.Add(fromNode); + return false; + } + + var fromGridCoords = new int2(fromNode.XCoordinateInGrid, fromNode.ZCoordinateInGrid); + var toGridCoords = new int2(toNode.XCoordinateInGrid, toNode.ZCoordinateInGrid); + + var fixedFrom = new int2(fromGridCoords.x*FixedPrecisionScale, fromGridCoords.y*FixedPrecisionScale) + fixedNormalizedFromPoint; + var fixedTo = new int2(toGridCoords.x*FixedPrecisionScale, toGridCoords.y*FixedPrecisionScale) + fixedNormalizedToPoint; + var dir = fixedTo - fixedFrom; + + int remainingSteps = System.Math.Abs(fromGridCoords.x - toGridCoords.x) + System.Math.Abs(fromGridCoords.y - toGridCoords.y); + if (continuePastEnd) remainingSteps = int.MaxValue; + + // If the from and to points are identical, but we start and end on different nodes, then dir will be zero + // and the direction calculations below will get a bit messsed up. + // So instead we don't take any steps at all, there's some code right at the end of this function which will + // look around the corner and find the target node anyway. + if (math.all(fixedFrom == fixedTo)) remainingSteps = 0; + + /* Y/Z + * | + * quadrant | quadrant + * 1 0 + * 2 + * | + * ---- 3 - X - 1 ----- X + * | + * 0 + * quadrant quadrant + * 2 | 3 + * | + */ + + // Calculate the quadrant index as shown in the diagram above (the axes are part of the quadrants after them in the counter clockwise direction) + int quadrant = 0; + + // The linecast line may be axis aligned, but we might still need to move to the side one step. + // Like in the following two cases (starting at node S at corner X and ending at node T at corner P). + // ┌─┬─┬─┬─┐ ┌─┬─┬─┬─┐ + // │S│ │ │ │ │S│ │#│T│ + // ├─X===P─┤ ├─X===P─┤ + // │ │ │ │T│ │ │ │ │ │ + // └─┴─┴─┴─┘ └─┴─┴─┴─┘ + // + // We make sure that we will always be able to move to the side of the line the target is on, if we happen to be on the wrong side of the line. + var dirBiased = dir; + if (dirBiased.x == 0) dirBiased.x = System.Math.Sign(FixedPrecisionScale/2 - fixedNormalizedToPoint.x); + if (dirBiased.y == 0) dirBiased.y = System.Math.Sign(FixedPrecisionScale/2 - fixedNormalizedToPoint.y); + + if (dirBiased.x <= 0 && dirBiased.y > 0) quadrant = 1; + else if (dirBiased.x < 0 && dirBiased.y <= 0) quadrant = 2; + else if (dirBiased.x >= 0 && dirBiased.y < 0) quadrant = 3; + + // This will be (1,2) for quadrant 0 and (2,3) for quadrant 1 etc. + // & 0x3 is just the same thing as % 4 but it is faster + // This is the direction which moves further to the right of the segment (when looking from the start) + int directionToReduceError = (quadrant + 1) & 0x3; + // This is the direction which moves further to the left of the segment (when looking from the start) + int directionToIncreaseError = (quadrant + 2) & 0x3; + + // All errors used in this function are proportional to the signed distance. + // They have a common multiplier which is dir.magnitude, but dividing away that would be very slow. + // Note that almost all errors are multiplied by 2. It might seem like this could be optimized away, + // but it cannot. The reason is that later when we use primaryDirectionError we only walk *half* a normal step. + // But we don't want to use division, so instead we multiply all other errors by 2. + // + // How much further we move away from (or towards) the line when walking along the primary direction (e.g up and right or down and left). + long primaryDirectionError = CrossMagnitude(dir, + new int2( + neighbourXOffsets[directionToIncreaseError]+neighbourXOffsets[directionToReduceError], + neighbourZOffsets[directionToIncreaseError]+neighbourZOffsets[directionToReduceError] + ) + ); + + // Conceptually we start with error 0 at 'fixedFrom' (i.e. precisely on the line). + // Imagine walking from fixedFrom to the center of the starting node. + // This will change our "error" (signed distance to the line) correspondingly. + int2 offset = new int2(FixedPrecisionScale/2, FixedPrecisionScale/2) - fixedNormalizedFromPoint; + + // Signed distance from the line (or at least a value proportional to that) + long error = CrossMagnitude(dir, offset) * 2 / FixedPrecisionScale; + + // Walking one step along the X axis will increase (or decrease) our error by this amount. + // This is equivalent to a cross product of dir with the x axis: CrossMagnitude(dir, new int2(1, 0)) * 2 + long xerror = -dir.y * 2; + // Walking one step along the Z axis will increase our error by this amount + long zerror = dir.x * 2; + + // When we move across a diagonal it can sometimes be important which side of the diagonal we prioritize. + // + // ┌───┬───┐ + // │ │ S │ + //=======P─C + // │ │ T │ + // └───┴───┘ + // + // Assume we are at node S and our target is node T at point P (it lies precisely between S and T). + // Note that the linecast line (illustrated as ===) comes from the left. This means that this case will be detected as a diagonal move (because corner C lies on the line). + // In this case we can walk either to the right from S or downwards. However walking to the right would mean that we end up in the wrong node (not the T node). + // Therefore we make sure that, if possible, we are on the same side of the linecast line as the center of the target node is. + int symmetryBreakingDirection1 = directionToIncreaseError; + int symmetryBreakingDirection2 = directionToReduceError; + + var fixedCenterOfToNode = new int2(toGridCoords.x*FixedPrecisionScale, toGridCoords.y*FixedPrecisionScale) + new int2(FixedPrecisionScale/2, FixedPrecisionScale/2); + long targetNodeError = CrossMagnitude(dir, fixedCenterOfToNode - fixedFrom); + if (targetNodeError < 0) { + symmetryBreakingDirection1 = directionToReduceError; + symmetryBreakingDirection2 = directionToIncreaseError; + } + + GridNodeBase prevNode = null; + GridNodeBase preventBacktrackingTo = null; + + for (; remainingSteps > 0; remainingSteps--) { + if (trace != null) trace.Add(fromNode); + + // How does the error change we take one half step in the primary direction. + // The point which this represents is a corner of the current node. + // Depending on which side of this point the line is (when seen from the center of the current node) + // we know which direction we should walk from the node. + // Since the error is just a signed distance, checking the side is equivalent to checking if its positive or negative. + var nerror = error + primaryDirectionError; + + int ndir; + GridNodeBase nextNode; + + if (nerror == 0) { + // This would be a diagonal move. But we don't allow those for simplicity (we can just as well just take it in two axis aligned steps). + // In this case we are free to choose which direction to move. + // If one direction is blocked, we choose the other one. + ndir = symmetryBreakingDirection1; + nextNode = fromNode.GetNeighbourAlongDirection(ndir); + if ((filter != null && nextNode != null && !filter(nextNode)) || nextNode == prevNode) nextNode = null; + + if (nextNode == null) { + // Try the other one too... + ndir = symmetryBreakingDirection2; + nextNode = fromNode.GetNeighbourAlongDirection(ndir); + if ((filter != null && nextNode != null && !filter(nextNode)) || nextNode == prevNode) nextNode = null; + } + } else { + // This is the happy-path of the linecast. We just move in the direction of the line. + // Check if we need to reduce or increase the error (we want to keep it near zero) + // and pick the appropriate direction to move in + ndir = nerror < 0 ? directionToIncreaseError : directionToReduceError; + nextNode = fromNode.GetNeighbourAlongDirection(ndir); + + // Use the filter + if ((filter != null && nextNode != null && !filter(nextNode)) || nextNode == prevNode) nextNode = null; + } + + // If we cannot move forward from this node, we might still be able to by side-stepping. + // This is a case that we need to handle if the linecast line exits this node at a corner. + // + // Assume we start at node S (at corner X) and linecast to node T (corner P) + // The linecast goes exactly between two rows of nodes. + // The code will start by going down one row, but then after a few nodes it hits an obstacle (when it's in node A). + // We don't want to report a hit here because the linecast only touches the edge of the obstacle, which is allowed. + // Instead we try to move to the node on the other side of the line (node B). + // The shared corner C lies exactly on the line, and we can detect that to figure out which neighbor we should move to. + // + // ┌───────┬───────┬───────┬───────┐ + // │ │ B │ │ │ + // │ S │ ┌───┼───────┼───┐ │ + // │ │ │ │ │ │ │ │ + // X===│=======│===C=======P───┼───┤ + // │ │ │ │ │#######│ │ │ + // │ └───┼───┘ │#######│ ▼ │ + // │ │ A │#######│ T │ + // └───────┴───────┴───────┴───────┘ + // + // After we have done this maneuver it is important that in the next step we don't try to move back to the node we came from. + // We keep track of this using the prevNode variable. + // + if (nextNode == null) { + // Loop over the two corners of the side of the node that we hit + for (int i = -1; i <= 1; i += 2) { + var d = (ndir + i + 4) & 0x3; + if (error + xerror/2 * (neighbourXOffsets[ndir] + neighbourXOffsets[d]) + zerror/2 * (neighbourZOffsets[ndir]+neighbourZOffsets[d]) == 0) { + // The line touches this corner precisely + // Try to side-step in that direction. + + nextNode = fromNode.GetNeighbourAlongDirection(d); + if ((filter != null && nextNode != null && !filter(nextNode)) || nextNode == prevNode || nextNode == preventBacktrackingTo) nextNode = null; + + if (nextNode != null) { + // This side-stepping might add 1 additional step to the path, or not. It's hard to say. + // We add 1 because the for loop will decrement remainingSteps after this iteration ends. + remainingSteps = 1 + System.Math.Abs(nextNode.XCoordinateInGrid - toGridCoords.x) + System.Math.Abs(nextNode.ZCoordinateInGrid - toGridCoords.y); + ndir = d; + prevNode = fromNode; + preventBacktrackingTo = nextNode; + } + break; + } + } + + // If we still have not found the next node yet, then we have hit an obstacle + if (nextNode == null) { + hit = new GridHitInfo { + node = fromNode, + direction = ndir, + }; + return true; + } + } + + // Calculate how large our error will be after moving along the given direction + error += xerror * neighbourXOffsets[ndir] + zerror * neighbourZOffsets[ndir]; + fromNode = nextNode; + } + + hit = new GridHitInfo { + node = fromNode, + direction = -1, + }; + + if (fromNode != toNode) { + // When the destination is on a corner it is sometimes possible that we end up in the wrong node. + // + // ┌───┬───┐ + // │ S │ │ + // ├───P───┤ + // │ T │ │ + // └───┴───┘ + // + // Assume we are at node S and our target is node T at point P (i.e. normalizedToPoint = (1,1) so it is in the corner of the node). + // In this case we can walk either to the right from S or downwards. However walking to the right would mean that we end up in the wrong node (not the T node). + // + // Similarly, if the connection from S to T was blocked for some reason (but both S and T are walkable), then we would definitely end up to the right of S, not in T. + // + // Therefore we check if the destination is a corner, and if so, try to reach all 4 nodes around that corner to see if any one of those is the destination. + var dirToDestination = fixedTo - (new int2(fromNode.XCoordinateInGrid, fromNode.ZCoordinateInGrid)*FixedPrecisionScale + new int2(FixedPrecisionScale/2, FixedPrecisionScale/2)); + + // Check if the destination is a corner of this node + if (math.all(math.abs(dirToDestination) == new int2(FixedPrecisionScale/2, FixedPrecisionScale/2))) { + var delta = dirToDestination*2/FixedPrecisionScale; + // Figure out which directions will move us towards the target node. + // We first try to move around the corner P in the counter-clockwise direction. + // And if that fails, we try to move in the clockwise direction. + // ┌───────┬───────┐ + // │ │ │ + // │ ccw◄─┼───S │ + // │ │ │ │ + // ├───────P───┼───┤ + // │ │ ▼ │ + // │ T │ cw │ + // │ │ │ + // └───────┴───────┘ + var counterClockwiseDirection = -1; + for (int i = 0; i < 4; i++) { + // Exactly one direction will satisfy this. It's kinda annnoying to calculate analytically. + if (neighbourXOffsets[i]+neighbourXOffsets[(i+1)&0x3] == delta.x && neighbourZOffsets[i] + neighbourZOffsets[(i+1)&0x3] == delta.y) { + counterClockwiseDirection = i; + break; + } + } + + int traceLength = trace != null ? trace.Count : 0; + int d = counterClockwiseDirection; + var node = fromNode; + for (int i = 0; i < 3 && node != toNode; i++) { + if (trace != null) trace.Add(node); + node = node.GetNeighbourAlongDirection(d); + if (node == null || (filter != null && !filter(node))) { + node = null; + break; + } + d = (d + 1) & 0x3; + } + + if (node != toNode) { + if (trace != null) trace.RemoveRange(traceLength, trace.Count - traceLength); + node = fromNode; + // Try the clockwise direction instead + d = (counterClockwiseDirection + 1) & 0x3; + for (int i = 0; i < 3 && node != toNode; i++) { + if (trace != null) trace.Add(node); + node = node.GetNeighbourAlongDirection(d); + if (node == null || (filter != null && !filter(node))) { + node = null; + break; + } + d = (d - 1 + 4) & 0x3; + } + + if (node != toNode && trace != null) { + trace.RemoveRange(traceLength, trace.Count - traceLength); + } + } + + fromNode = node; + } + } + + if (trace != null) trace.Add(fromNode); + return fromNode != toNode; + } + + protected override void SerializeExtraInfo (GraphSerializationContext ctx) { + if (nodes == null) { + ctx.writer.Write(-1); + return; + } + + ctx.writer.Write(nodes.Length); + + for (int i = 0; i < nodes.Length; i++) { + nodes[i].SerializeNode(ctx); + } + + SerializeNodeSurfaceNormals(ctx); + } + + protected override void DeserializeExtraInfo (GraphSerializationContext ctx) { + int count = ctx.reader.ReadInt32(); + + if (count == -1) { + nodes = null; + return; + } + + nodes = new GridNode[count]; + + for (int i = 0; i < nodes.Length; i++) { + nodes[i] = newGridNodeDelegate(); + active.InitializeNode(nodes[i]); + nodes[i].DeserializeNode(ctx); + } + DeserializeNativeData(ctx, ctx.meta.version >= AstarSerializer.V4_3_6); + } + + protected void DeserializeNativeData (GraphSerializationContext ctx, bool normalsSerialized) { + UpdateTransform(); + var tracker = ObjectPool.Claim(); + bool layeredDataLayout = this is LayerGridGraph; + var nodeArraySize = new int3(width, LayerCount, depth); + nodeData = GridGraphNodeData.ReadFromNodes(nodes, new Slice3D(nodeArraySize, new IntBounds(0, nodeArraySize)), default, default, Allocator.Persistent, layeredDataLayout, tracker); + nodeData.PersistBuffers(tracker); + DeserializeNodeSurfaceNormals(ctx, nodes, !normalsSerialized); + tracker.AllWritesDependency.Complete(); + ObjectPool.Release(ref tracker); + } + + protected void SerializeNodeSurfaceNormals (GraphSerializationContext ctx) { + var normals = nodeData.normals.AsUnsafeReadOnlySpan(); + for (int i = 0; i < nodes.Length; i++) { + ctx.SerializeVector3(new Vector3(normals[i].x, normals[i].y, normals[i].z)); + } + } + + protected void DeserializeNodeSurfaceNormals (GraphSerializationContext ctx, GridNodeBase[] nodes, bool ignoreForCompatibility) { + if (nodeData.normals.IsCreated) nodeData.normals.Dispose(); + nodeData.normals = new NativeArray(nodes.Length, Allocator.Persistent, NativeArrayOptions.UninitializedMemory); + if (ignoreForCompatibility) { + // For backwards compatibility with older versions that do not have the information stored. + // For most of these versions the #maxStepUsesSlope field will be deserialized to false anyway, so this array will not have any effect. + for (int i = 0; i < nodes.Length; i++) { + // If a node is null (can only happen for layered grid graphs) then the normal must be set to zero. + // Otherwise we set it to a "reasonable" up direction. + nodeData.normals[i] = nodes[i] != null ? new float4(0, 1, 0, 0) : float4.zero; + } + } else { + for (int i = 0; i < nodes.Length; i++) { + var v = ctx.DeserializeVector3(); + nodeData.normals[i] = new float4(v.x, v.y, v.z, 0); + } + } + } + + void HandleBackwardsCompatibility (GraphSerializationContext ctx) { + // For compatibility + if (ctx.meta.version <= AstarSerializer.V4_3_2) maxStepUsesSlope = false; + +#pragma warning disable CS0618 // Type or member is obsolete + if (penaltyPosition) { + penaltyPosition = false; + // Can't convert it exactly. So assume there are no nodes with an elevation greater than 1000 + rules.AddRule(new RuleElevationPenalty { + penaltyScale = Int3.Precision * penaltyPositionFactor * 1000.0f, + elevationRange = new Vector2(-penaltyPositionOffset/Int3.Precision, -penaltyPositionOffset/Int3.Precision + 1000), + curve = AnimationCurve.Linear(0, 0, 1, 1), + }); + } + + if (penaltyAngle) { + penaltyAngle = false; + + // Approximate the legacy behavior with an animation curve + var curve = AnimationCurve.Linear(0, 0, 1, 1); + var keys = new Keyframe[7]; + for (int i = 0; i < keys.Length; i++) { + var angle = Mathf.PI*0.5f*i/(keys.Length-1); + var penalty = (1F-Mathf.Pow(Mathf.Cos(angle), penaltyAnglePower))*penaltyAngleFactor; + var key = new Keyframe(Mathf.Rad2Deg * angle, penalty); + keys[i] = key; + } + var maxPenalty = keys.Max(k => k.value); + if (maxPenalty > 0) for (int i = 0; i < keys.Length; i++) keys[i].value /= maxPenalty; + curve.keys = keys; + for (int i = 0; i < keys.Length; i++) { + curve.SmoothTangents(i, 0.5f); + } + + rules.AddRule(new RuleAnglePenalty { + penaltyScale = maxPenalty, + curve = curve, + }); + } + + if (textureData.enabled) { + textureData.enabled = false; + var channelScales = textureData.factors.Select(x => x/255.0f).ToList(); + while (channelScales.Count < 4) channelScales.Add(1000); + var channels = textureData.channels.Cast().ToList(); + while (channels.Count < 4) channels.Add(RuleTexture.ChannelUse.None); + + rules.AddRule(new RuleTexture { + texture = textureData.source, + channels = channels.ToArray(), + channelScales = channelScales.ToArray(), + scalingMode = RuleTexture.ScalingMode.FixedScale, + nodesPerPixel = 1.0f, + }); + } +#pragma warning restore CS0618 // Type or member is obsolete + } + + protected override void PostDeserialization (GraphSerializationContext ctx) { + HandleBackwardsCompatibility(ctx); + + UpdateTransform(); + SetUpOffsetsAndCosts(); + GridNode.SetGridGraph((int)graphIndex, this); + + // Deserialize all nodes + if (nodes == null || nodes.Length == 0) return; + + if (width*depth != nodes.Length) { + Debug.LogError("Node data did not match with bounds data. Probably a change to the bounds/width/depth data was made after scanning the graph just prior to saving it. Nodes will be discarded"); + nodes = new GridNodeBase[0]; + return; + } + + for (int z = 0; z < depth; z++) { + for (int x = 0; x < width; x++) { + var node = nodes[z*width+x]; + + if (node == null) { + Debug.LogError("Deserialization Error : Couldn't cast the node to the appropriate type - GridGenerator"); + return; + } + + node.NodeInGridIndex = z*width+x; + } + } + } + } + + /// + /// Number of neighbours for a single grid node. + /// Since: The 'Six' item was added in 3.6.1 + /// + public enum NumNeighbours { + Four, + Eight, + Six + } + + /// Information about a linecast hit on a grid graph + public struct GridHitInfo { + /// + /// The node which contained the edge that was hit. + /// This may be null in case no particular edge was hit. + /// + public GridNodeBase node; + /// + /// Direction from the node to the edge that was hit. + /// This will be in the range of 0 to 4 (exclusive) or -1 if no particular edge was hit. + /// + /// See: + /// + public int direction; + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/GridGraph.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/GridGraph.cs.meta new file mode 100644 index 0000000..e7eeee2 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/GridGraph.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: d5d28978e568e40429b2981fab3e380e +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LayerGridGraph.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LayerGridGraph.cs new file mode 100644 index 0000000..c629224 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LayerGridGraph.cs @@ -0,0 +1,246 @@ +#if !ASTAR_NO_GRID_GRAPH +using UnityEngine; +using System.Collections.Generic; +using Pathfinding.Serialization; +using Pathfinding.Graphs.Grid; + +namespace Pathfinding { + /// + /// Grid Graph, supports layered worlds. + /// [Open online documentation to see images] + /// The GridGraph is great in many ways, reliable, easily configured and updatable during runtime. + /// But it lacks support for worlds which have multiple layers, such as a building with multiple floors. + /// That's where this graph type comes in. It supports basically the same stuff as the grid graph, but also multiple layers. + /// It uses a bit more memory than a regular grid graph, but is otherwise equivalent. + /// + /// [Open online documentation to see images] + /// + /// Note: The graph supports 16 layers by default, but it can be increased to 256 by enabling the ASTAR_LEVELGRIDNODE_MORE_LAYERS option in the A* Inspector -> Settings -> Optimizations tab. + /// + /// See: + /// + [Pathfinding.Util.Preserve] + public class LayerGridGraph : GridGraph, IUpdatableGraph { + // This function will be called when this graph is destroyed + protected override void DisposeUnmanagedData () { + base.DisposeUnmanagedData(); + + // Clean up a reference in a static variable which otherwise should point to this graph forever and stop the GC from collecting it + LevelGridNode.ClearGridGraph((int)graphIndex, this); + } + + public LayerGridGraph () { + newGridNodeDelegate = () => new LevelGridNode(); + } + + protected override GridNodeBase[] AllocateNodesJob (int size, out Unity.Jobs.JobHandle dependency) { + var newNodes = new LevelGridNode[size]; + + dependency = active.AllocateNodes(newNodes, size, newGridNodeDelegate, 1); + return newNodes; + } + + /// + /// Number of layers. + /// Warning: Do not modify this variable + /// + [JsonMember] + internal int layerCount; + + /// Nodes with a short distance to the node above it will be set unwalkable + [JsonMember] + public float characterHeight = 0.4F; + + internal int lastScannedWidth; + internal int lastScannedDepth; + + public override int LayerCount { + get => layerCount; + protected set => layerCount = value; + } + + public override int MaxLayers => LevelGridNode.MaxLayerCount; + + public override int CountNodes () { + if (nodes == null) return 0; + + int counter = 0; + for (int i = 0; i < nodes.Length; i++) { + if (nodes[i] != null) counter++; + } + return counter; + } + + public override void GetNodes (System.Action action) { + if (nodes == null) return; + + for (int i = 0; i < nodes.Length; i++) { + if (nodes[i] != null) action(nodes[i]); + } + } + + /// + /// Get all nodes in a rectangle. + /// Returns: The number of nodes written to the buffer. + /// + /// Region in which to return nodes. It will be clamped to the grid. + /// Buffer in which the nodes will be stored. Should be at least as large as the number of nodes that can exist in that region. + public override int GetNodesInRegion (IntRect rect, GridNodeBase[] buffer) { + // Clamp the rect to the grid + // Rect which covers the whole grid + var gridRect = new IntRect(0, 0, width-1, depth-1); + + rect = IntRect.Intersection(rect, gridRect); + + if (nodes == null || !rect.IsValid() || nodes.Length != width*depth*layerCount) return 0; + + int counter = 0; + try { + for (int l = 0; l < layerCount; l++) { + var lwd = l * Width * Depth; + for (int z = rect.ymin; z <= rect.ymax; z++) { + var offset = lwd + z*Width; + for (int x = rect.xmin; x <= rect.xmax; x++) { + var node = nodes[offset + x]; + if (node != null) { + buffer[counter] = node; + counter++; + } + } + } + } + } catch (System.IndexOutOfRangeException) { + // Catch the exception which 'buffer[counter] = node' would throw if the buffer was too small + throw new System.ArgumentException("Buffer is too small"); + } + + return counter; + } + + /// + /// Node in the specified cell. + /// Returns null if the coordinate is outside the grid. + /// + /// If you know the coordinate is inside the grid and you are looking to maximize performance then you + /// can look up the node in the internal array directly which is slightly faster. + /// See: + /// + public GridNodeBase GetNode (int x, int z, int layer) { + if (x < 0 || z < 0 || x >= width || z >= depth || layer < 0 || layer >= layerCount) return null; + return nodes[x + z*width + layer*width*depth]; + } + + protected override IGraphUpdatePromise ScanInternal (bool async) { + LevelGridNode.SetGridGraph((int)graphIndex, this); + layerCount = 0; + lastScannedWidth = width; + lastScannedDepth = depth; + return base.ScanInternal(async); + } + + protected override GridNodeBase GetNearestFromGraphSpace (Vector3 positionGraphSpace) { + if (nodes == null || depth*width*layerCount != nodes.Length) { + return null; + } + + float xf = positionGraphSpace.x; + float zf = positionGraphSpace.z; + int x = Mathf.Clamp((int)xf, 0, width-1); + int z = Mathf.Clamp((int)zf, 0, depth-1); + var worldPos = transform.Transform(positionGraphSpace); + return GetNearestNode(worldPos, x, z, null); + } + + private GridNodeBase GetNearestNode (Vector3 position, int x, int z, NNConstraint constraint) { + int index = width*z+x; + float minDist = float.PositiveInfinity; + GridNodeBase minNode = null; + + for (int i = 0; i < layerCount; i++) { + var node = nodes[index + width*depth*i]; + if (node != null) { + float dist = ((Vector3)node.position - position).sqrMagnitude; + if (dist < minDist && (constraint == null || constraint.Suitable(node))) { + minDist = dist; + minNode = node; + } + } + } + return minNode; + } + + protected override void SerializeExtraInfo (GraphSerializationContext ctx) { + if (nodes == null) { + ctx.writer.Write(-1); + return; + } + + ctx.writer.Write(nodes.Length); + + for (int i = 0; i < nodes.Length; i++) { + if (nodes[i] == null) { + ctx.writer.Write(-1); + } else { + ctx.writer.Write(0); + nodes[i].SerializeNode(ctx); + } + } + + SerializeNodeSurfaceNormals(ctx); + } + + protected override void DeserializeExtraInfo (GraphSerializationContext ctx) { + int count = ctx.reader.ReadInt32(); + + if (count == -1) { + nodes = null; + return; + } + + nodes = new LevelGridNode[count]; + for (int i = 0; i < nodes.Length; i++) { + if (ctx.reader.ReadInt32() != -1) { + nodes[i] = newGridNodeDelegate(); + active.InitializeNode(nodes[i]); + nodes[i].DeserializeNode(ctx); + } else { + nodes[i] = null; + } + } + DeserializeNativeData(ctx, ctx.meta.version >= AstarSerializer.V4_3_37); + } + + protected override void PostDeserialization (GraphSerializationContext ctx) { + UpdateTransform(); + lastScannedWidth = width; + lastScannedDepth = depth; + + SetUpOffsetsAndCosts(); + LevelGridNode.SetGridGraph((int)graphIndex, this); + + if (nodes == null || nodes.Length == 0) return; + + if (width*depth*layerCount != nodes.Length) { + Debug.LogError("Node data did not match with bounds data. Probably a change to the bounds/width/depth data was made after scanning the graph, just prior to saving it. Nodes will be discarded"); + nodes = new GridNodeBase[0]; + return; + } + + for (int i = 0; i < layerCount; i++) { + for (int z = 0; z < depth; z++) { + for (int x = 0; x < width; x++) { + LevelGridNode node = nodes[z*width+x + width*depth*i] as LevelGridNode; + + if (node == null) { + continue; + } + + node.NodeInGridIndex = z*width+x; + node.LayerCoordinateInGrid = i; + } + } + } + } + } +} +#endif diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LayerGridGraph.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LayerGridGraph.cs.meta new file mode 100644 index 0000000..d897588 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LayerGridGraph.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: d6ea17a678e4042de89cdfa01860ad8a +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LinkGraph.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LinkGraph.cs new file mode 100644 index 0000000..7212135 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LinkGraph.cs @@ -0,0 +1,167 @@ +using UnityEngine; +using System.Collections.Generic; +using Pathfinding.Serialization; +using Pathfinding.Util; +using UnityEngine.Assertions; + +namespace Pathfinding { + using System; + using Pathfinding.Drawing; + using Unity.Jobs; + + /// + /// Graph for off-mesh links. + /// + /// This is an internal graph type which is used to store off-mesh links. + /// An off-mesh link between two nodes A and B is represented as: A <--> N1 <--> N2 <--> B . + /// where N1 and N2 are two special nodes added to this graph at the exact start and endpoints of the link. + /// + /// This graph is not persistent. So it will never be saved to disk and a new one will be created each time the game starts. + /// + /// It is also not possible to query for the nearest node in this graph. The method will always return an empty result. + /// This is by design, as all pathfinding should start on the navmesh, not on an off-mesh link. + /// + /// See: + /// See: + /// + [JsonOptIn] + [Pathfinding.Util.Preserve] + public class LinkGraph : NavGraph { + LinkNode[] nodes = new LinkNode[0]; + int nodeCount; + + public override bool isScanned => true; + + public override bool persistent => false; + + public override bool showInInspector => false; + + public override int CountNodes() => nodeCount; + + protected override void DestroyAllNodes () { + base.DestroyAllNodes(); + nodes = new LinkNode[0]; + nodeCount = 0; + } + + public override void GetNodes (Action action) { + if (nodes == null) return; + for (int i = 0; i < nodeCount; i++) action(nodes[i]); + } + + internal LinkNode AddNode () { + AssertSafeToUpdateGraph(); + if (nodeCount >= nodes.Length) { + Memory.Realloc(ref nodes, Mathf.Max(16, nodeCount * 2)); + } + nodeCount++; + return nodes[nodeCount-1] = new LinkNode(active) { + nodeInGraphIndex = nodeCount - 1, + GraphIndex = graphIndex, + Walkable = true, + }; + } + + internal void RemoveNode (LinkNode node) { + if (nodes[node.nodeInGraphIndex] != node) throw new ArgumentException("Node is not in this graph"); + // Remove and swap with the last node + nodeCount--; + nodes[node.nodeInGraphIndex] = nodes[nodeCount]; + nodes[node.nodeInGraphIndex].nodeInGraphIndex = node.nodeInGraphIndex; + nodes[nodeCount] = null; + node.Destroy(); + } + + public override float NearestNodeDistanceSqrLowerBound(Vector3 position, NNConstraint constraint = null) => float.PositiveInfinity; + + /// + /// It's not possible to query for the nearest node in a link graph. + /// This method will always return an empty result. + /// + public override NNInfo GetNearest(Vector3 position, NNConstraint constraint, float maxDistanceSqr) => default; + + public override void OnDrawGizmos (DrawingData gizmos, bool drawNodes, RedrawScope redrawScope) { + // We rely on the link components themselves to draw the links + + // TODO + base.OnDrawGizmos(gizmos, drawNodes, redrawScope); + } + + class LinkGraphUpdatePromise : IGraphUpdatePromise { + public LinkGraph graph; + + public void Apply (IGraphUpdateContext ctx) { + // Destroy all previous nodes (if any) + graph.DestroyAllNodes(); + } + + public IEnumerator Prepare() => null; + } + + protected override IGraphUpdatePromise ScanInternal () => new LinkGraphUpdatePromise { graph = this }; + } + + public class LinkNode : PointNode { + public OffMeshLinks.OffMeshLinkSource linkSource; + public OffMeshLinks.OffMeshLinkConcrete linkConcrete; + public int nodeInGraphIndex; + + public LinkNode() { } + public LinkNode(AstarPath active) : base(active) {} + + public override void RemovePartialConnection (GraphNode node) { + linkConcrete.staleConnections = true; + // Mark the link as dirty so that it will be recalculated. + // Ensure that this does not immediately schedule an update. + // Nodes should only be updated during work items and while graphs are scanned, + // and in those cases node links will be refreshed anyway. + // However, this can also trigger when the AstarPath component is being destroyed, + // or when a graph is removed. In those cases, we don't want to schedule an update. + AstarPath.active.offMeshLinks.DirtyNoSchedule(linkSource); + base.RemovePartialConnection(node); + } + + public override void Open (Path path, uint pathNodeIndex, uint gScore) { + // Note: Not calling path.OpenCandidateConnectionsToEndNode here, because link nodes should never be the end node of a path + + if (connections == null) return; + + var pathHandler = (path as IPathInternals).PathHandler; + var pn = pathHandler.pathNodes[pathNodeIndex]; + // Check if our parent node was also a link node by checking if it is in the same graph as this node. + // If it is, we are allowed to connect to non-link nodes. + // Otherwise, we are at the start of the link and we must only connect to other link nodes. + // This is to avoid the path going to a link node, and then going directly back to a non-link node + // without actually traversing the link. It would technically be a valid path, + // but it causes confusion for other scripts that look for off-mesh links in the path. + // TODO: Store the other link node as a field to be able to do a more robust check here? + var isEndOfLink = !pathHandler.IsTemporaryNode(pn.parentIndex) && pathHandler.GetNode(pn.parentIndex).GraphIndex == GraphIndex; + var canTraverseNonLinkNodes = isEndOfLink; + + for (int i = 0; i < connections.Length; i++) { + GraphNode other = connections[i].node; + + if (canTraverseNonLinkNodes == (other.GraphIndex != GraphIndex) && path.CanTraverse(this, other)) { + if (other is PointNode) { + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, connections[i].cost, 0, other.position); + } else { + // When connecting to a non-link node, use a special function to open the connection. + // The typical case for this is that we are at the end of an off-mesh link and we are connecting to a navmesh node. + // In that case, this node's position is in the interior of the navmesh node. We let the navmesh node decide how + // that should be handled. + other.OpenAtPoint(path, pathNodeIndex, position, gScore); + } + } + } + } + + public override void OpenAtPoint (Path path, uint pathNodeIndex, Int3 pos, uint gScore) { + if (path.CanTraverse(this)) { + // Note: Not calling path.OpenCandidateConnectionsToEndNode here, because link nodes should never be the end node of a path + + var cost = (uint)(pos - this.position).costMagnitude; + path.OpenCandidateConnection(pathNodeIndex, NodeIndex, gScore, cost, 0, position); + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LinkGraph.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LinkGraph.cs.meta new file mode 100644 index 0000000..b15371b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/LinkGraph.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: ea4fb279f02f6c74aaf36b080fa54d28 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavGraph.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavGraph.cs new file mode 100644 index 0000000..7be91ca --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavGraph.cs @@ -0,0 +1,469 @@ +using UnityEngine; +using System.Collections.Generic; +using Pathfinding.Util; +using Pathfinding.Serialization; +using Unity.Collections; + +namespace Pathfinding { + using Pathfinding.Drawing; + + /// + /// Exposes internal methods for graphs. + /// This is used to hide methods that should not be used by any user code + /// but still have to be 'public' or 'internal' (which is pretty much the same as 'public' + /// as this library is distributed with source code). + /// + /// Hiding the internal methods cleans up the documentation and IntelliSense suggestions. + /// + public interface IGraphInternals { + string SerializedEditorSettings { get; set; } + void OnDestroy(); + void DisposeUnmanagedData(); + void DestroyAllNodes(); + IGraphUpdatePromise ScanInternal(bool async); + void SerializeExtraInfo(GraphSerializationContext ctx); + void DeserializeExtraInfo(GraphSerializationContext ctx); + void PostDeserialization(GraphSerializationContext ctx); + } + + /// Base class for all graphs + public abstract class NavGraph : IGraphInternals { + /// Reference to the AstarPath object in the scene + public AstarPath active; + + /// + /// Used as an ID of the graph, considered to be unique. + /// Note: This is Pathfinding.Util.Guid not System.Guid. A replacement for System.Guid was coded for better compatibility with iOS + /// + [JsonMember] + public Guid guid; + + /// + /// Default penalty to apply to all nodes. + /// + /// See: graph-updates (view in online documentation for working links) + /// See: + /// See: tags (view in online documentation for working links) + /// + [JsonMember] + public uint initialPenalty; + + /// Is the graph open in the editor + [JsonMember] + public bool open; + + /// Index of the graph, used for identification purposes + public uint graphIndex; + + /// + /// Name of the graph. + /// Can be set in the unity editor + /// + [JsonMember] + public string name; + + /// + /// Enable to draw gizmos in the Unity scene view. + /// In the inspector this value corresponds to the state of + /// the 'eye' icon in the top left corner of every graph inspector. + /// + [JsonMember] + public bool drawGizmos = true; + + /// + /// Used in the editor to check if the info screen is open. + /// Should be inside UNITY_EDITOR only \ but just in case anyone tries to serialize a NavGraph instance using Unity, I have left it like this as it would otherwise cause a crash when building. + /// Version 3.0.8.1 was released because of this bug only + /// + [JsonMember] + public bool infoScreenOpen; + + /// Used in the Unity editor to store serialized settings for graph inspectors + [JsonMember] + string serializedEditorSettings; + + + /// True if the graph exists, false if it has been destroyed + internal bool exists => active != null; + + /// + /// True if the graph has been scanned and contains nodes. + /// + /// Graphs are typically scanned when the game starts, but they can also be scanned manually. + /// + /// If a graph has not been scanned, it does not contain any nodes and it not possible to use it for pathfinding. + /// + /// See: + /// + public abstract bool isScanned { get; } + + /// + /// True if the graph will be included when serializing graph data. + /// + /// If false, the graph will be ignored when saving graph data. + /// + /// Most graphs are persistent, but the is not persistent because links are always re-created from components at runtime. + /// + public virtual bool persistent => true; + + /// + /// True if the graph should be visible in the editor. + /// + /// False is used for some internal graph types that users don't have to worry about. + /// + public virtual bool showInInspector => true; + + /// + /// World bounding box for the graph. + /// + /// This always contains the whole graph. + /// + /// Note: Since this an axis aligned bounding box, it may not be particularly tight if the graph is rotated. + /// + /// It is ok for a graph type to return an infinitely large bounding box, but this may make some operations less efficient. + /// The point graph will always return an infinitely large bounding box. + /// + public virtual Bounds bounds => new Bounds(Vector3.zero, Vector3.positiveInfinity); + + /// + /// Number of nodes in the graph. + /// Note that this is, unless the graph type has overriden it, an O(n) operation. + /// + /// This is an O(1) operation for grid graphs and point graphs. + /// For layered grid graphs it is an O(n) operation. + /// + public virtual int CountNodes () { + int count = 0; + + GetNodes(_ => count++); + return count; + } + + /// Calls a delegate with all nodes in the graph until the delegate returns false + public void GetNodes (System.Func action) { + bool cont = true; + + GetNodes(node => { + if (cont) cont &= action(node); + }); + } + + /// + /// Calls a delegate with all nodes in the graph. + /// This is the primary way of iterating through all nodes in a graph. + /// + /// Do not change the graph structure inside the delegate. + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// + /// gg.GetNodes(node => { + /// // Here is a node + /// Debug.Log("I found a node at position " + (Vector3)node.position); + /// }); + /// + /// + /// If you want to store all nodes in a list you can do this + /// + /// + /// var gg = AstarPath.active.data.gridGraph; + /// + /// List nodes = new List(); + /// + /// gg.GetNodes((System.Action)nodes.Add); + /// + /// + /// See: + /// + public abstract void GetNodes(System.Action action); + + /// + /// True if the point is inside the bounding box of this graph. + /// + /// This method may be able to use a tighter (non-axis aligned) bounding box than using the one returned by . + /// + /// It is valid for a graph to return true for all points in the world. + /// In particular the PointGraph will always return true, since it has no limits on its bounding box. + /// + public virtual bool IsInsideBounds(Vector3 point) => true; + + /// + /// Throws an exception if it is not safe to update internal graph data right now. + /// + /// It is safe to update graphs when graphs are being scanned, or inside a work item. + /// In other cases pathfinding could be running at the same time, which would not appreciate graph data changing under its feet. + /// + /// See: + /// + protected void AssertSafeToUpdateGraph () { + if (!active.IsAnyWorkItemInProgress && !active.isScanning) { + throw new System.Exception("Trying to update graphs when it is not safe to do so. Graph updates must be done inside a work item or when a graph is being scanned. See AstarPath.AddWorkItem"); + } + } + + /// + /// Notifies the system that changes have been made inside these bounds. + /// + /// This should be called by graphs whenever they are changed. + /// It will cause off-mesh links to be updated, and it will also ensure events are callled. + /// + /// The bounding box should cover the surface of all nodes that have been updated. + /// It is fine to use a larger bounding box than necessary (even an infinite one), though this may be slower, since more off-mesh links need to be recalculated. + /// You can even use an infinitely large bounding box if you don't want to bother calculating a more accurate one. + /// You can also call this multiple times to dirty multiple bounding boxes. + /// + /// When scanning the graph, this is called automatically - with the value from the property - for all graphs after the scanning has finished. + /// + /// Note: If possible, it is recommended to use or instead of this method. + /// They currently do the same thing, but that may change in future versions. + /// + protected void DirtyBounds(Bounds bounds) => active.DirtyBounds(bounds); + + /// + /// Moves the nodes in this graph. + /// Multiplies all node positions by deltaMatrix. + /// + /// For example if you want to move all your nodes in e.g a point graph 10 units along the X axis from the initial position + /// + /// var graph = AstarPath.data.pointGraph; + /// var m = Matrix4x4.TRS (new Vector3(10,0,0), Quaternion.identity, Vector3.one); + /// graph.RelocateNodes (m); + /// + /// + /// Note: For grid graphs, navmesh graphs and recast graphs it is recommended to + /// use their custom overloads of the RelocateNodes method which take parameters + /// for e.g center and nodeSize (and additional parameters) instead since + /// they are both easier to use and are less likely to mess up pathfinding. + /// + /// Warning: This method is lossy for PointGraphs, so calling it many times may + /// cause node positions to lose precision. For example if you set the scale + /// to 0 in one call then all nodes will be scaled/moved to the same point and + /// you will not be able to recover their original positions. The same thing + /// happens for other - less extreme - values as well, but to a lesser degree. + /// + public virtual void RelocateNodes (Matrix4x4 deltaMatrix) { + GetNodes(node => node.position = ((Int3)deltaMatrix.MultiplyPoint((Vector3)node.position))); + } + + /// + /// Lower bound on the squared distance from the given point to the closest node in this graph. + /// + /// This is used to speed up searching for the closest node when there is more than one graph in the scene, + /// by checking the graphs in order of increasing estimated distance to the point. + /// + /// Implementors may return 0 at all times if it is hard to come up with a good lower bound. + /// It is more important that this function is fast than that it is accurate. + /// + /// The position to check from + /// A constraint on which nodes are valid. This may or may not be used by the function. You may pass null if you consider all nodes valid. + public virtual float NearestNodeDistanceSqrLowerBound (Vector3 position, NNConstraint constraint = null) { + // If the graph doesn't provide a way to calculate a lower bound, just return 0, since that is always a valid lower bound + return 0; + } + + /// + /// Returns the nearest node to a position using the specified NNConstraint. + /// + /// The returned will contain both the closest node, and the closest point on the surface of that node. + /// The distance is measured to the closest point on the surface of the node. + /// + /// See: You can use instead, if you want to check all graphs. + /// + /// Version: Before 4.3.63, this method would not use the NNConstraint in all cases. + /// + /// The position to try to find the closest node to. + /// Used to limit which nodes are considered acceptable. + /// You may, for example, only want to consider walkable nodes. + /// If null, all nodes will be considered acceptable. + public NNInfo GetNearest (Vector3 position, NNConstraint constraint = null) { + var maxDistanceSqr = constraint == null || constraint.constrainDistance ? active.maxNearestNodeDistanceSqr : float.PositiveInfinity; + return GetNearest(position, constraint, maxDistanceSqr); + } + + /// + /// Nearest node to a position using the specified NNConstraint. + /// + /// The returned will contain both the closest node, and the closest point on the surface of that node. + /// The distance is measured to the closest point on the surface of the node. + /// + /// See: You can use instead, if you want to check all graphs. + /// + /// The position to try to find the closest node to. + /// Used to limit which nodes are considered acceptable. + /// You may, for example, only want to consider walkable nodes. + /// If null, all nodes will be considered acceptable. + /// The maximum squared distance from the position to the node. + /// If the node is further away than this, the function will return an empty NNInfo. + /// You may pass infinity if you do not want to limit the distance. + public virtual NNInfo GetNearest (Vector3 position, NNConstraint constraint, float maxDistanceSqr) { + // This is a default implementation and it is pretty slow + // Graphs usually override this to provide faster and more specialised implementations + + float minDistSqr = maxDistanceSqr; + GraphNode minNode = null; + + // Loop through all nodes and find the closest suitable node + GetNodes(node => { + float dist = (position-(Vector3)node.position).sqrMagnitude; + + if (dist < minDistSqr && (constraint == null || constraint.Suitable(node))) { + minDistSqr = dist; + minNode = node; + } + }); + + return minNode != null ? new NNInfo(minNode, (Vector3)minNode.position, minDistSqr) : NNInfo.Empty; + } + + /// + /// Returns the nearest node to a position using the specified NNConstraint. + /// Deprecated: Use GetNearest instead + /// + [System.Obsolete("Use GetNearest instead")] + public NNInfo GetNearestForce (Vector3 position, NNConstraint constraint) { + return GetNearest(position, constraint); + } + + /// + /// Function for cleaning up references. + /// This will be called on the same time as OnDisable on the gameObject which the AstarPath script is attached to (remember, not in the editor). + /// Use for any cleanup code such as cleaning up static variables which otherwise might prevent resources from being collected. + /// Use by creating a function overriding this one in a graph class, but always call base.OnDestroy () in that function. + /// All nodes should be destroyed in this function otherwise a memory leak will arise. + /// + protected virtual void OnDestroy () { + DestroyAllNodes(); + DisposeUnmanagedData(); + } + + /// + /// Cleans up any unmanaged data that the graph has. + /// Note: The graph has to stay valid after this. However it need not be in a scanned state. + /// + protected virtual void DisposeUnmanagedData () { + } + + /// + /// Destroys all nodes in the graph. + /// Warning: This is an internal method. Unless you have a very good reason, you should probably not call it. + /// + protected virtual void DestroyAllNodes () { + GetNodes(node => node.Destroy()); + } + + /// + /// Captures a snapshot of a part of the graph, to allow restoring it later. + /// + /// See: for more details + /// + /// If this graph type does not support taking snapshots, or if the bounding box does not intersect with the graph, this method returns null. + /// + public virtual IGraphSnapshot Snapshot(Bounds bounds) => null; + + /// + /// Scan the graph. + /// + /// Consider using AstarPath.Scan() instead since this function only scans this graph, and if you are using multiple graphs + /// with connections between them, then it is better to scan all graphs at once. + /// + public void Scan () { + active.Scan(this); + } + + /// + /// Internal method to scan the graph. + /// + /// Deprecated: You should use ScanInternal(bool) instead. + /// + protected virtual IGraphUpdatePromise ScanInternal () { + throw new System.NotImplementedException(); + } + + /// + /// Internal method to scan the graph. + /// Override this function to implement custom scanning logic. + /// + protected virtual IGraphUpdatePromise ScanInternal (bool async) { + return ScanInternal(); + } + + /// + /// Serializes graph type specific node data. + /// This function can be overriden to serialize extra node information (or graph information for that matter) + /// which cannot be serialized using the standard serialization. + /// Serialize the data in any way you want and return a byte array. + /// When loading, the exact same byte array will be passed to the DeserializeExtraInfo function. + /// These functions will only be called if node serialization is enabled. + /// + protected virtual void SerializeExtraInfo (GraphSerializationContext ctx) { + } + + /// + /// Deserializes graph type specific node data. + /// See: SerializeExtraInfo + /// + protected virtual void DeserializeExtraInfo (GraphSerializationContext ctx) { + } + + /// + /// Called after all deserialization has been done for all graphs. + /// Can be used to set up more graph data which is not serialized + /// + protected virtual void PostDeserialization (GraphSerializationContext ctx) { + } + + /// Draw gizmos for the graph + public virtual void OnDrawGizmos (DrawingData gizmos, bool drawNodes, RedrawScope redrawScope) { + if (!drawNodes) { + return; + } + + // This is a relatively slow default implementation. + // subclasses of the base graph class may override + // this method to draw gizmos in a more optimized way + + var hasher = new NodeHasher(active); + GetNodes(node => hasher.HashNode(node)); + + // Update the gizmo mesh if necessary + if (!gizmos.Draw(hasher, redrawScope)) { + using (var helper = GraphGizmoHelper.GetGizmoHelper(gizmos, active, hasher, redrawScope)) { + GetNodes((System.Action)helper.DrawConnections); + } + } + + if (active.showUnwalkableNodes) DrawUnwalkableNodes(gizmos, active.unwalkableNodeDebugSize, redrawScope); + } + + protected void DrawUnwalkableNodes (DrawingData gizmos, float size, RedrawScope redrawScope) { + var hasher = DrawingData.Hasher.Create(this); + + GetNodes(node => { + hasher.Add(node.Walkable); + if (!node.Walkable) hasher.Add(node.position); + }); + + if (!gizmos.Draw(hasher, redrawScope)) { + using (var builder = gizmos.GetBuilder(hasher)) { + using (builder.WithColor(AstarColor.UnwalkableNode)) { + GetNodes(node => { + if (!node.Walkable) builder.SolidBox((Vector3)node.position, new Unity.Mathematics.float3(size, size, size)); + }); + } + } + } + } + + #region IGraphInternals implementation + string IGraphInternals.SerializedEditorSettings { get { return serializedEditorSettings; } set { serializedEditorSettings = value; } } + void IGraphInternals.OnDestroy() => OnDestroy(); + void IGraphInternals.DisposeUnmanagedData() => DisposeUnmanagedData(); + void IGraphInternals.DestroyAllNodes() => DestroyAllNodes(); + IGraphUpdatePromise IGraphInternals.ScanInternal(bool async) => ScanInternal(async); + void IGraphInternals.SerializeExtraInfo(GraphSerializationContext ctx) => SerializeExtraInfo(ctx); + void IGraphInternals.DeserializeExtraInfo(GraphSerializationContext ctx) => DeserializeExtraInfo(ctx); + void IGraphInternals.PostDeserialization(GraphSerializationContext ctx) => PostDeserialization(ctx); + + #endregion + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavGraph.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavGraph.cs.meta new file mode 100644 index 0000000..dda38a3 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavGraph.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: c7636164485c04efe8fad73ab1ee985f +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavMeshGraph.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavMeshGraph.cs new file mode 100644 index 0000000..8f9cc04 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavMeshGraph.cs @@ -0,0 +1,372 @@ +using UnityEngine; +using System.Collections.Generic; + +namespace Pathfinding { + using UnityEngine.Profiling; + using Pathfinding.Util; + using Pathfinding.Serialization; + using Unity.Collections; + using Unity.Jobs; + using Pathfinding.Graphs.Navmesh.Jobs; + using Pathfinding.Graphs.Navmesh; + using Unity.Mathematics; + + public interface INavmesh { + void GetNodes(System.Action del); + } + + /// + /// Generates graphs based on navmeshes. + /// [Open online documentation to see images] + /// + /// Navmeshes are meshes in which each triangle defines a walkable area. + /// These are great because the AI can get so much more information on how it can walk. + /// Polygons instead of points mean that the can produce really nice looking paths, and the graphs are also really fast to search + /// and have a low memory footprint because fewer nodes are usually needed to describe the same area compared to grid graphs. + /// + /// The navmesh graph requires that you create a navmesh manually. The package also has support for generating navmeshes automatically using the . + /// + /// For a tutorial on how to configure a navmesh graph, take a look at getstarted2 (view in online documentation for working links). + /// + /// [Open online documentation to see images] + /// [Open online documentation to see images] + /// + /// See: Pathfinding.RecastGraph + /// + [JsonOptIn] + [Pathfinding.Util.Preserve] + public class NavMeshGraph : NavmeshBase, IUpdatableGraph { + /// Mesh to construct navmesh from + [JsonMember] + public Mesh sourceMesh; + + /// Offset in world space + [JsonMember] + public Vector3 offset; + + /// Rotation in degrees + [JsonMember] + public Vector3 rotation; + + /// Scale of the graph + [JsonMember] + public float scale = 1; + + /// + /// Determines how normals are calculated. + /// Disable for spherical graphs or other complicated surfaces that allow the agents to e.g walk on walls or ceilings. + /// + /// By default the normals of the mesh will be flipped so that they point as much as possible in the upwards direction. + /// The normals are important when connecting adjacent nodes. Two adjacent nodes will only be connected if they are oriented the same way. + /// This is particularly important if you have a navmesh on the walls or even on the ceiling of a room. Or if you are trying to make a spherical navmesh. + /// If you do one of those things then you should set disable this setting and make sure the normals in your source mesh are properly set. + /// + /// If you for example take a look at the image below. In the upper case then the nodes on the bottom half of the + /// mesh haven't been connected with the nodes on the upper half because the normals on the lower half will have been + /// modified to point inwards (as that is the direction that makes them face upwards the most) while the normals on + /// the upper half point outwards. This causes the nodes to not connect properly along the seam. When this option + /// is set to false instead the nodes are connected properly as in the original mesh all normals point outwards. + /// [Open online documentation to see images] + /// + /// The default value of this field is true to reduce the risk for errors in the common case. If a mesh is supplied that + /// has all normals pointing downwards and this option is false, then some methods like will not work correctly + /// as they assume that the normals point upwards. For a more complicated surface like a spherical graph those methods make no sense anyway + /// as there is no clear definition of what it means to be "inside" a triangle when there is no clear up direction. + /// + [JsonMember] + public bool recalculateNormals = true; + + /// + /// Cached bounding box minimum of . + /// This is important when the graph has been saved to a file and is later loaded again, but the original mesh does not exist anymore (or has been moved). + /// In that case we still need to be able to find the bounding box since the method uses it. + /// + [JsonMember] + Vector3 cachedSourceMeshBoundsMin; + + /// + /// Radius to use when expanding navmesh cuts. + /// + /// See: + /// + [JsonMember] + public float navmeshCuttingCharacterRadius = 0.5f; + + public override float NavmeshCuttingCharacterRadius => navmeshCuttingCharacterRadius; + + public override bool RecalculateNormals => recalculateNormals; + + public override float TileWorldSizeX => forcedBoundsSize.x; + + public override float TileWorldSizeZ => forcedBoundsSize.z; + + // Tiles are not supported, so this is irrelevant + public override float MaxTileConnectionEdgeDistance => 0f; + + /// + /// True if the point is inside the bounding box of this graph. + /// + /// Warning: If your input mesh is entirely flat, the bounding box will also end up entirely flat (with a height of zero), this will make this function return false for almost all points, unless they are at exactly the right y-coordinate. + /// + /// Note: For an unscanned graph, this will always return false. + /// + public override bool IsInsideBounds (Vector3 point) { + if (this.tiles == null || this.tiles.Length == 0 || sourceMesh == null) return false; + + var local = transform.InverseTransform(point); + var size = sourceMesh.bounds.size*scale; + + // Allow a small margin + const float EPS = 0.0001f; + + return local.x >= -EPS && local.y >= -EPS && local.z >= -EPS && local.x <= size.x + EPS && local.y <= size.y + EPS && local.z <= size.z + EPS; + } + + /// + /// World bounding box for the graph. + /// + /// This always contains the whole graph. + /// + /// Note: Since this is an axis-aligned bounding box, it may not be particularly tight if the graph is significantly rotated. + /// + /// If no mesh has been assigned, this will return a zero sized bounding box at the origin. + /// + /// [Open online documentation to see images] + /// + public override Bounds bounds { + get { + if (sourceMesh == null) return default; + var m = (float4x4)CalculateTransform().matrix; + var b = new ToWorldMatrix(new float3x3(m.c0.xyz, m.c1.xyz, m.c2.xyz)).ToWorld(new Bounds(Vector3.zero, sourceMesh.bounds.size * scale)); + return b; + } + } + + public override GraphTransform CalculateTransform () { + return new GraphTransform(Matrix4x4.TRS(offset, Quaternion.Euler(rotation), Vector3.one) * Matrix4x4.TRS(sourceMesh != null ? sourceMesh.bounds.min * scale : cachedSourceMeshBoundsMin * scale, Quaternion.identity, Vector3.one)); + } + + class NavMeshGraphUpdatePromise : IGraphUpdatePromise { + public NavMeshGraph graph; + public List graphUpdates; + + public void Apply (IGraphUpdateContext ctx) { + for (int i = 0; i < graphUpdates.Count; i++) { + var graphUpdate = graphUpdates[i]; + UpdateArea(graphUpdate, graph); + // TODO: Not strictly accurate, since the update may affect node that have a surface that extends + // outside of the bounds. + ctx.DirtyBounds(graphUpdate.bounds); + } + } + } + + IGraphUpdatePromise IUpdatableGraph.ScheduleGraphUpdates (List graphUpdates) => new NavMeshGraphUpdatePromise { graph = this, graphUpdates = graphUpdates }; + + public static void UpdateArea (GraphUpdateObject o, INavmeshHolder graph) { + Bounds bounds = graph.transform.InverseTransform(o.bounds); + + // Bounding rectangle with integer coordinates + var irect = new IntRect( + Mathf.FloorToInt(bounds.min.x*Int3.Precision), + Mathf.FloorToInt(bounds.min.z*Int3.Precision), + Mathf.CeilToInt(bounds.max.x*Int3.Precision), + Mathf.CeilToInt(bounds.max.z*Int3.Precision) + ); + + // Corners of the bounding rectangle + var a = new Int3(irect.xmin, 0, irect.ymin); + var b = new Int3(irect.xmin, 0, irect.ymax); + var c = new Int3(irect.xmax, 0, irect.ymin); + var d = new Int3(irect.xmax, 0, irect.ymax); + + var ymin = ((Int3)bounds.min).y; + var ymax = ((Int3)bounds.max).y; + + // Loop through all nodes and check if they intersect the bounding box + graph.GetNodes(_node => { + var node = _node as TriangleMeshNode; + + bool inside = false; + + int allLeft = 0; + int allRight = 0; + int allTop = 0; + int allBottom = 0; + + // Check bounding box rect in XZ plane + for (int v = 0; v < 3; v++) { + Int3 p = node.GetVertexInGraphSpace(v); + + if (irect.Contains(p.x, p.z)) { + inside = true; + break; + } + + if (p.x < irect.xmin) allLeft++; + if (p.x > irect.xmax) allRight++; + if (p.z < irect.ymin) allTop++; + if (p.z > irect.ymax) allBottom++; + } + + if (!inside && (allLeft == 3 || allRight == 3 || allTop == 3 || allBottom == 3)) { + return; + } + + // Check if the polygon edges intersect the bounding rect + for (int v = 0; v < 3; v++) { + int v2 = v > 1 ? 0 : v+1; + + Int3 vert1 = node.GetVertexInGraphSpace(v); + Int3 vert2 = node.GetVertexInGraphSpace(v2); + + if (VectorMath.SegmentsIntersectXZ(a, b, vert1, vert2)) { inside = true; break; } + if (VectorMath.SegmentsIntersectXZ(a, c, vert1, vert2)) { inside = true; break; } + if (VectorMath.SegmentsIntersectXZ(c, d, vert1, vert2)) { inside = true; break; } + if (VectorMath.SegmentsIntersectXZ(d, b, vert1, vert2)) { inside = true; break; } + } + + // Check if the node contains any corner of the bounding rect + if (inside || node.ContainsPointInGraphSpace(a) || node.ContainsPointInGraphSpace(b) || node.ContainsPointInGraphSpace(c) || node.ContainsPointInGraphSpace(d)) { + inside = true; + } + + if (!inside) { + return; + } + + int allAbove = 0; + int allBelow = 0; + + // Check y coordinate + for (int v = 0; v < 3; v++) { + Int3 p = node.GetVertexInGraphSpace(v); + if (p.y < ymin) allBelow++; + if (p.y > ymax) allAbove++; + } + + // Polygon is either completely above the bounding box or completely below it + if (allBelow == 3 || allAbove == 3) return; + + // Triangle is inside the bounding box! + // Update it! + o.WillUpdateNode(node); + o.Apply(node); + }); + } + + class NavMeshGraphScanPromise : IGraphUpdatePromise { + public NavMeshGraph graph; + bool emptyGraph; + GraphTransform transform; + NavmeshTile[] tiles; + Vector3 forcedBoundsSize; + IntRect tileRect; + + public IEnumerator Prepare () { + var sourceMesh = graph.sourceMesh; + graph.cachedSourceMeshBoundsMin = sourceMesh != null ? sourceMesh.bounds.min : Vector3.zero; + transform = graph.CalculateTransform(); + + if (sourceMesh == null) { + emptyGraph = true; + yield break; + } + + if (!sourceMesh.isReadable) { + Debug.LogError("The source mesh " + sourceMesh.name + " is not readable. Enable Read/Write in the mesh's import settings.", sourceMesh); + emptyGraph = true; + yield break; + } + + Profiler.BeginSample("GetMeshData"); + var meshDatas = Mesh.AcquireReadOnlyMeshData(sourceMesh); + MeshUtility.GetMeshData(meshDatas, 0, out var vertices, out var indices); + meshDatas.Dispose(); + Profiler.EndSample(); + + // Convert the vertices to graph space + // so that the minimum of the bounding box of the mesh is at the origin + // (the vertices will later be transformed to world space) + var meshToGraphSpace = Matrix4x4.TRS(-sourceMesh.bounds.min * graph.scale, Quaternion.identity, Vector3.one * graph.scale); + + var promise = JobBuildTileMeshFromVertices.Schedule(vertices, indices, meshToGraphSpace, graph.RecalculateNormals); + forcedBoundsSize = sourceMesh.bounds.size * graph.scale; + tileRect = new IntRect(0, 0, 0, 0); + tiles = new NavmeshTile[tileRect.Area]; + var tilesGCHandle = System.Runtime.InteropServices.GCHandle.Alloc(tiles); + var tileWorldSize = new Vector2(forcedBoundsSize.x, forcedBoundsSize.z); + var tileNodeConnections = new NativeArray(tiles.Length, Allocator.Persistent); + var calculateConnectionsJob = new JobCalculateTriangleConnections { + tileMeshes = promise.GetValue().tiles, + nodeConnections = tileNodeConnections, + }.Schedule(promise.handle); + var createTilesJob = new JobCreateTiles { + tileMeshes = promise.GetValue().tiles, + tiles = tilesGCHandle, + tileRect = tileRect, + graphTileCount = new Int2(tileRect.Width, tileRect.Height), + graphIndex = graph.graphIndex, + initialPenalty = graph.initialPenalty, + recalculateNormals = graph.recalculateNormals, + graphToWorldSpace = transform.matrix, + tileWorldSize = tileWorldSize, + }.Schedule(promise.handle); + var applyConnectionsJob = new JobWriteNodeConnections { + tiles = tilesGCHandle, + nodeConnections = tileNodeConnections, + }.Schedule(JobHandle.CombineDependencies(createTilesJob, calculateConnectionsJob)); + + yield return applyConnectionsJob; + + var navmeshOutput = promise.Complete(); + // This has already been used in the createTilesJob + navmeshOutput.Dispose(); + tileNodeConnections.Dispose(); + + vertices.Dispose(); + indices.Dispose(); + + tilesGCHandle.Free(); + } + + public void Apply (IGraphUpdateContext ctx) { + if (emptyGraph) { + graph.forcedBoundsSize = Vector3.zero; + graph.transform = transform; + graph.tileZCount = graph.tileXCount = 1; + TriangleMeshNode.SetNavmeshHolder(AstarPath.active.data.GetGraphIndex(graph), graph); + graph.FillWithEmptyTiles(); + return; + } + + // Destroy all previous nodes (if any) + graph.DestroyAllNodes(); + + // Initialize all nodes that were created in the jobs + for (int j = 0; j < tiles.Length; j++) AstarPath.active.InitializeNodes(tiles[j].nodes); + + // Assign all data as one atomic operation (from the viewpoint of the main thread) + graph.forcedBoundsSize = forcedBoundsSize; + graph.transform = transform; + graph.tileXCount = tileRect.Width; + graph.tileZCount = tileRect.Height; + graph.tiles = tiles; + TriangleMeshNode.SetNavmeshHolder(graph.active.data.GetGraphIndex(graph), graph); + + // Signal that tiles have been recalculated to the navmesh cutting system. + graph.navmeshUpdateData.OnRecalculatedTiles(tiles); + if (graph.OnRecalculatedTiles != null) graph.OnRecalculatedTiles(tiles.Clone() as NavmeshTile[]); + } + } + + protected override IGraphUpdatePromise ScanInternal (bool async) => new NavMeshGraphScanPromise { graph = this }; + + protected override void PostDeserialization (GraphSerializationContext ctx) { + if (ctx.meta.version < AstarSerializer.V4_3_74) { + this.navmeshCuttingCharacterRadius = 0; + } + base.PostDeserialization(ctx); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavMeshGraph.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavMeshGraph.cs.meta new file mode 100644 index 0000000..1b6d4ad --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavMeshGraph.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: ffae751ca240c466185a168f4a9836cb +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh.meta new file mode 100644 index 0000000..0baa22d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Navmesh.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 85b3765d770e3e34ab31b2c0b95043f5 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: 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: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavmeshBase.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavmeshBase.cs new file mode 100644 index 0000000..cce316b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavmeshBase.cs @@ -0,0 +1,1736 @@ +using UnityEngine; +using System.Collections.Generic; +using UnityEngine.Profiling; +using Unity.Collections; +using Unity.Mathematics; +using Unity.Burst; +using UnityEngine.Assertions; + +namespace Pathfinding { + using System.IO; + using Pathfinding.Util; + using Pathfinding.Serialization; + using Math = System.Math; + using System.Linq; + using Pathfinding.Drawing; + using Pathfinding.Graphs.Navmesh; + + /// Base class for and + [BurstCompile] + public abstract class NavmeshBase : NavGraph, INavmesh, INavmeshHolder, ITransformedGraph + , IRaycastableGraph { +#if ASTAR_RECAST_LARGER_TILES + // Larger tiles + public const int VertexIndexMask = 0xFFFFF; + + public const int TileIndexMask = 0x7FF; + public const int TileIndexOffset = 20; +#else + // Larger worlds + public const int VertexIndexMask = 0xFFF; + + public const int TileIndexMask = 0x7FFFF; + public const int TileIndexOffset = 12; +#endif + + /// Size of the bounding box. + [JsonMember] + public Vector3 forcedBoundsSize = new Vector3(100, 40, 100); + + public abstract float NavmeshCuttingCharacterRadius { get; } + + /// Size of a tile in world units along the X axis + public abstract float TileWorldSizeX { get; } + + /// Size of a tile in world units along the Z axis + public abstract float TileWorldSizeZ { get; } + + /// + /// Maximum (vertical) distance between the sides of two nodes for them to be connected across a tile edge. + /// When tiles are connected to each other, the nodes sometimes do not line up perfectly + /// so some allowance must be made to allow tiles that do not match exactly to be connected with each other. + /// + public abstract float MaxTileConnectionEdgeDistance { get; } + + /// Show an outline of the polygons in the Unity Editor + [JsonMember] + public bool showMeshOutline = true; + + /// Show the connections between the polygons in the Unity Editor + [JsonMember] + public bool showNodeConnections; + + /// Show the surface of the navmesh + [JsonMember] + public bool showMeshSurface = true; + + /// Number of tiles along the X-axis + public int tileXCount; + /// Number of tiles along the Z-axis + public int tileZCount; + + /// + /// All tiles. + /// + /// See: + /// + protected NavmeshTile[] tiles; + + /// + /// Perform nearest node searches in XZ space only. + /// Recomended for single-layered environments. Faster but can be inaccurate esp. in multilayered contexts. + /// You should not use this if the graph is rotated since then the XZ plane no longer corresponds to the ground plane. + /// + /// This can be important on sloped surfaces. See the image below in which the closest point for each blue point is queried for: + /// [Open online documentation to see images] + /// + /// You can also control this using a . + /// + /// Deprecated: Set the appropriate fields on the NNConstraint instead. + /// + [JsonMember] + [System.Obsolete("Set the appropriate fields on the NNConstraint instead")] + public bool nearestSearchOnlyXZ; + + /// + /// Should navmesh cuts affect this graph. + /// See: + /// + [JsonMember] + public bool enableNavmeshCutting = true; + + /// + /// Handles navmesh cutting. + /// See: + /// See: + /// + public readonly NavmeshUpdates.NavmeshUpdateSettings navmeshUpdateData; + + /// Currently updating tiles in a batch + bool batchTileUpdate; + + /// List of tiles updating during batch + List batchUpdatedTiles = new List(); + + /// List of nodes that are going to be destroyed as part of a batch update + List batchNodesToDestroy = new List(); + + /// + /// Determines how the graph transforms graph space to world space. + /// See: + /// + /// Warning: Do not modify this directly, instead use e.g. + /// + public GraphTransform transform = GraphTransform.identityTransform; + + GraphTransform ITransformedGraph.transform { get { return transform; } } + + /// \copydoc Pathfinding::NavMeshGraph::recalculateNormals + public abstract bool RecalculateNormals { get; } + + public override bool isScanned => tiles != null; + + /// + /// Returns a new transform which transforms graph space to world space. + /// Does not update the field. + /// See: + /// + public abstract GraphTransform CalculateTransform(); + + /// + /// Called when tiles have been completely recalculated. + /// This is called after scanning the graph and after + /// performing graph updates that completely recalculate tiles + /// (not ones that simply modify e.g penalties). + /// It is not called after NavmeshCut updates. + /// + public System.Action OnRecalculatedTiles; + + /// + /// Tile at the specified x, z coordinate pair. + /// The first tile is at (0,0), the last tile at (tileXCount-1, tileZCount-1). + /// + /// + /// var graph = AstarPath.active.data.recastGraph; + /// int tileX = 5; + /// int tileZ = 8; + /// NavmeshTile tile = graph.GetTile(tileX, tileZ); + /// + /// for (int i = 0; i < tile.nodes.Length; i++) { + /// // ... + /// } + /// // or you can access the nodes like this: + /// tile.GetNodes(node => { + /// // ... + /// }); + /// + /// + public NavmeshTile GetTile (int x, int z) { + return tiles[x + z * tileXCount]; + } + + /// + /// Vertex coordinate for the specified vertex index. + /// + /// Throws: IndexOutOfRangeException if the vertex index is invalid. + /// Throws: NullReferenceException if the tile the vertex is in is not calculated. + /// + /// See: NavmeshTile.GetVertex + /// + public Int3 GetVertex (int index) { + int tileIndex = (index >> TileIndexOffset) & TileIndexMask; + + return tiles[tileIndex].GetVertex(index); + } + + /// Vertex coordinate in graph space for the specified vertex index + public Int3 GetVertexInGraphSpace (int index) { + int tileIndex = (index >> TileIndexOffset) & TileIndexMask; + + return tiles[tileIndex].GetVertexInGraphSpace(index); + } + + /// Tile index from a vertex index + public static int GetTileIndex (int index) { + return (index >> TileIndexOffset) & TileIndexMask; + } + + public int GetVertexArrayIndex (int index) { + return index & VertexIndexMask; + } + + /// Tile coordinates from a tile index + public void GetTileCoordinates (int tileIndex, out int x, out int z) { + //z = System.Math.DivRem (tileIndex, tileXCount, out x); + z = tileIndex/tileXCount; + x = tileIndex - z*tileXCount; + } + + /// + /// All tiles. + /// Warning: Do not modify this array + /// + public NavmeshTile[] GetTiles () { + return tiles; + } + + /// + /// Returns a bounds object with the bounding box of a group of tiles. + /// + /// The bounding box is defined in world space. + /// + /// Tiles to get the bounding box of. The rectangle is in tile coordinates where 1 unit = 1 tile. + public Bounds GetTileBounds (IntRect rect) { + return GetTileBounds(rect.xmin, rect.ymin, rect.Width, rect.Height); + } + + /// + /// Returns a bounds object with the bounding box of a group of tiles. + /// The bounding box is defined in world space. + /// + public Bounds GetTileBounds (int x, int z, int width = 1, int depth = 1) { + return transform.Transform(GetTileBoundsInGraphSpace(x, z, width, depth)); + } + + /// Returns an XZ bounds object with the bounds of a group of tiles in graph space. + /// Tiles to get the bounding box of. The rectangle is in tile coordinates where 1 unit = 1 tile. + public Bounds GetTileBoundsInGraphSpace (IntRect rect) { + return GetTileBoundsInGraphSpace(rect.xmin, rect.ymin, rect.Width, rect.Height); + } + + /// 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 b = new Bounds(); + + b.SetMinMax( + new Vector3(x*TileWorldSizeX, 0, z*TileWorldSizeZ), + new Vector3((x+width)*TileWorldSizeX, forcedBoundsSize.y, (z+depth)*TileWorldSizeZ) + ); + return b; + } + + /// + /// Returns the tile coordinate which contains the specified position. + /// It is not necessarily a valid tile (i.e it could be out of bounds). + /// + public Int2 GetTileCoordinates (Vector3 position) { + position = transform.InverseTransform(position); + position.x /= TileWorldSizeX; + position.z /= TileWorldSizeZ; + return new Int2((int)position.x, (int)position.z); + } + + protected override void OnDestroy () { + base.OnDestroy(); + TriangleMeshNode.ClearNavmeshHolder((int)graphIndex, this); + } + + protected override void DestroyAllNodes () { + // Remove cross-graph connections + GetNodes(node => { + node.GetConnections(other => { + if (node.GraphIndex != other.GraphIndex) other.RemovePartialConnection(node); + }); + }); + // Destroy all nodes + GetNodes(node => { + node.Destroy(); + }); + + if (tiles != null) { + for (int i = 0; i < tiles.Length; i++) { + tiles[i].Dispose(); + } + tiles = null; + } + } + + public override void RelocateNodes (Matrix4x4 deltaMatrix) { + RelocateNodes(deltaMatrix * transform); + } + + /// + /// Moves the nodes in this graph. + /// Moves all the nodes in such a way that the specified transform is the new graph space to world space transformation for the graph. + /// You usually use this together with the method. + /// + /// So for example if you want to move and rotate all your nodes in e.g a recast graph you can do + /// + /// // Move the graph to the point (20, 10, 10), rotated 45 degrees around the X axis + /// var graph = AstarPath.active.data.recastGraph; + /// graph.forcedBoundsCenter = new Vector3(20, 10, 10); + /// graph.rotation = new Vector3(45, 0, 0); + /// graph.RelocateNodes(graph.CalculateTransform()); + /// + /// + /// For a navmesh graph it will look like: + /// * + /// // Move the graph to the point (20, 10, 10), rotated 45 degrees around the X axis + /// var graph = AstarPath.active.data.navmesh; + /// graph.offset = new Vector3(20, 10, 10); + /// graph.rotation = new Vector3(45, 0, 0); + /// graph.RelocateNodes(graph.CalculateTransform()); + /// + /// + /// This will move all the nodes to new positions as if the new graph settings had been there from the start. + /// + /// Note: RelocateNodes(deltaMatrix) is not equivalent to RelocateNodes(new GraphTransform(deltaMatrix)). + /// The overload which takes a matrix multiplies all existing node positions with the matrix while this + /// overload does not take into account the current positions of the nodes. + /// + /// See: + /// + public void RelocateNodes (GraphTransform newTransform) { + DirtyBounds(bounds); + transform = newTransform; + if (tiles != null) { + // Move all the vertices in each tile + for (int tileIndex = 0; tileIndex < tiles.Length; tileIndex++) { + var tile = tiles[tileIndex]; + if (tile != null) { + tile.vertsInGraphSpace.CopyTo(tile.verts); + // Transform the graph space vertices to world space + transform.Transform(tile.verts); + + for (int nodeIndex = 0; nodeIndex < tile.nodes.Length; nodeIndex++) { + tile.nodes[nodeIndex].UpdatePositionFromVertices(); + } + } + } + DirtyBounds(bounds); + } + } + + /// Creates a single new empty tile + protected NavmeshTile NewEmptyTile (int x, int z) { + return new NavmeshTile { + x = x, + z = z, + w = 1, + d = 1, + verts = default, + vertsInGraphSpace = default, + tris = default, + nodes = new TriangleMeshNode[0], + bbTree = default, + graph = this, + }; + } + + public override void GetNodes (System.Action action) { + if (tiles == null) return; + + for (int i = 0; i < tiles.Length; i++) { + if (tiles[i] == null || tiles[i].x+tiles[i].z*tileXCount != i) continue; + TriangleMeshNode[] nodes = tiles[i].nodes; + + if (nodes == null) continue; + + for (int j = 0; j < nodes.Length; j++) action(nodes[j]); + } + } + + /// + /// 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 + var r = 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)); + // Clamp to bounds + r = IntRect.Intersection(r, new IntRect(0, 0, tileXCount-1, tileZCount-1)); + return r; + } + + /// Returns a rect containing the indices of all tiles touching the specified bounds. + /// Graph space rectangle (in graph space all tiles are on the XZ plane regardless of graph rotation and other transformations, the first tile has a corner at the origin) + public IntRect GetTouchingTilesInGraphSpace (Rect rect) { + // Calculate world bounds of all affected tiles + var r = new IntRect(Mathf.FloorToInt(rect.xMin / TileWorldSizeX), Mathf.FloorToInt(rect.yMin / TileWorldSizeZ), Mathf.FloorToInt(rect.xMax / TileWorldSizeX), Mathf.FloorToInt(rect.yMax / TileWorldSizeZ)); + + // Clamp to bounds + r = IntRect.Intersection(r, new IntRect(0, 0, tileXCount-1, tileZCount-1)); + return r; + } + + /// + /// Returns a rect containing the indices of all tiles by rounding the specified bounds to tile borders. + /// This is different from GetTouchingTiles in that the tiles inside the rectangle returned from this method + /// may not contain the whole bounds, while that is guaranteed for GetTouchingTiles. + /// + [System.Obsolete("Use GetTouchingTiles instead. This method will be removed in a future update.")] + public IntRect GetTouchingTilesRound (Bounds bounds) { + bounds = transform.InverseTransform(bounds); + + //Calculate world bounds of all affected tiles + var r = new IntRect(Mathf.RoundToInt(bounds.min.x / TileWorldSizeX), Mathf.RoundToInt(bounds.min.z / TileWorldSizeZ), Mathf.RoundToInt(bounds.max.x / TileWorldSizeX)-1, Mathf.RoundToInt(bounds.max.z / TileWorldSizeZ)-1); + //Clamp to bounds + r = IntRect.Intersection(r, new IntRect(0, 0, tileXCount-1, tileZCount-1)); + return r; + } + + protected void ConnectTileWithNeighbours (NavmeshTile tile, bool onlyUnflagged = false) { + if (tile.w != 1 || tile.d != 1) { + throw new System.ArgumentException("Tile widths or depths other than 1 are not supported. The fields exist mainly for possible future expansions."); + } + + // Loop through z and x offsets to adjacent tiles + // _ x _ + // x _ x + // _ x _ + for (int zo = -1; zo <= 1; zo++) { + var z = tile.z + zo; + if (z < 0 || z >= tileZCount) continue; + + for (int xo = -1; xo <= 1; xo++) { + var x = tile.x + xo; + if (x < 0 || x >= tileXCount) continue; + + // Ignore diagonals and the tile itself + if ((xo == 0) == (zo == 0)) continue; + + var otherTile = tiles[x + z*tileXCount]; + if (!onlyUnflagged || !otherTile.flag) { + ConnectTiles(otherTile, tile, TileWorldSizeX, TileWorldSizeZ, MaxTileConnectionEdgeDistance); + } + } + } + } + + public override float NearestNodeDistanceSqrLowerBound (Vector3 position, NNConstraint constraint) { + if (tiles == null) return float.PositiveInfinity; + + var localPosition = (float3)transform.InverseTransform(position); + var projection = new BBTree.ProjectionParams(constraint, transform); + return projection.SquaredRectPointDistanceOnPlane(new IntRect(0, 0, (int)(Int3.Precision * tileXCount * TileWorldSizeX), (int)(Int3.Precision * tileZCount * TileWorldSizeZ)), localPosition); + } + + public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, float maxDistanceSqr) { + if (tiles == null) return NNInfo.Empty; + + var localPosition = (float3)transform.InverseTransform(position); + // Figure out tile coordinates of the point + var tx = (int)(localPosition.x / TileWorldSizeX); + var tz = (int)(localPosition.z / TileWorldSizeZ); + + // Clamp to graph borders + tx = Mathf.Clamp(tx, 0, tileXCount-1); + tz = Mathf.Clamp(tz, 0, tileZCount-1); + + int wmax = Math.Max(tileXCount, tileZCount); + + var best = NNInfo.Empty; + float bestDistanceSq = maxDistanceSqr; + var projection = new BBTree.ProjectionParams(constraint, transform); + + var tileSize = Math.Min(TileWorldSizeX, TileWorldSizeX); + // Search outwards in a diamond pattern from the closest tile + // 2 + // 2 1 2 + // 2 1 0 1 2 etc. + // 2 1 2 + // 2 + for (int w = 0; w < wmax; w++) { + int zmax = Math.Min(w+tz+1, tileZCount); + for (int z = Math.Max(-w+tz, 0); z < zmax; z++) { + // Solve for z such that abs(x-tx) + abs(z-tx) == w + // Delta X coordinate + int originalDx = Math.Abs(w - Math.Abs(z-tz)); + var dx = originalDx; + // Solution is dx + tx and -dx + tx + // This loop will first check +dx and then -dx + // If dx happens to be zero, then it will not run twice + do { + // Absolute x coordinate + int x = -dx + tx; + if (x >= 0 && x < tileXCount) { + NavmeshTile tile = tiles[x + z*tileXCount]; + + if (tile != null && tile.bbTree.DistanceSqrLowerBound(localPosition, in projection) <= bestDistanceSq) { + tile.bbTree.QueryClosest(localPosition, constraint, in projection, ref bestDistanceSq, ref best, tile.nodes, tile.tris, tile.vertsInGraphSpace); + } + } + + dx = -dx; + } while (dx != originalDx); + } + + // Stop the loop when we can guarantee that no nodes will be closer than the ones we have already searched. + // If the projection is not aligned with the graph's XZ plane, then we cannot guarantee this, and so we have to + // search all tiles in the graph. This is pretty inefficient, but you typically only use non-aligned projections + // when making spherical/non-planar worlds, and in those cases you typically use a non-tiled navmesh graph anyway. + // Note that even if distanceLimit=0 we should run at least one iteration of the loop. + var nextW = w+1; + var distanceThreshold = math.max(0, nextW-2)*tileSize; + if (projection.alignedWithXZPlane && bestDistanceSq - 0.00001f <= distanceThreshold*distanceThreshold) break; + } + + // Transform the closest point from graph space to world space + if (best.node != null) best = new NNInfo(best.node, transform.Transform(best.position), best.distanceCostSqr); + return best; + } + + /// + /// Finds the first node which contains position. + /// "Contains" is defined as position is inside the triangle node when seen from above. + /// In case of a multilayered environment, the closest node which contains the point is returned. + /// + /// Returns null if there was no node containing the point. This serves as a quick + /// check for "is this point on the navmesh or not". + /// + /// Note that the behaviour of this method is distinct from the GetNearest method. + /// The GetNearest method will return the closest node to a point, + /// which is not necessarily the one which contains it when seen from above. + /// + /// Uses to define the "up" direction. The up direction of the graph will be used if it is not set. + /// The up direction defines what "inside" a node means. A point is inside a node if it is inside the triangle when seen from above. + /// + /// See: + /// + public GraphNode PointOnNavmesh (Vector3 position, NNConstraint constraint) { + if (tiles == null) return null; + // TODO: Kinda ugly to modify the NNConstraint here + // This is not ideal, especially if the query is being done on a separate thread + constraint = constraint ?? NNConstraint.None; + var prevDistanceMetric = constraint.distanceMetric; + if (!constraint.distanceMetric.isProjectedDistance) { + constraint.distanceMetric = DistanceMetric.ClosestAsSeenFromAbove(); + } + constraint.distanceMetric.distanceScaleAlongProjectionDirection = 0; + var result = GetNearest(position, constraint, 0).node; + constraint.distanceMetric = prevDistanceMetric; + return result; + } + + /// Fills graph with tiles created by NewEmptyTile + protected void FillWithEmptyTiles () { + UnityEngine.Assertions.Assert.IsNull(tiles); + tiles = new NavmeshTile[tileXCount*tileZCount]; + + for (int z = 0; z < tileZCount; z++) { + for (int x = 0; x < tileXCount; x++) { + tiles[z*tileXCount + x] = NewEmptyTile(x, z); + } + } + } + + /// Create connections between all nodes + protected static void CreateNodeConnections (TriangleMeshNode[] nodes, bool keepExistingConnections) { + List connections = ListPool.Claim(); + + var nodeRefs = ObjectPoolSimple >.Claim(); + + nodeRefs.Clear(); + + // Build node neighbours + for (int i = 0; i < nodes.Length; i++) { + TriangleMeshNode node = nodes[i]; + + int av = node.GetVertexCount(); + + for (int a = 0; a < av; a++) { + // Recast can in some very special cases generate degenerate triangles which are simply lines + // In that case, duplicate keys might be added and thus an exception will be thrown + // It is safe to ignore the second edge though... I think (only found one case where this happens) + var key = new Int2(node.GetVertexIndex(a), node.GetVertexIndex((a+1) % av)); + nodeRefs.TryAdd(key, i); + } + } + + for (int i = 0; i < nodes.Length; i++) { + TriangleMeshNode node = nodes[i]; + + connections.Clear(); + if (keepExistingConnections && node.connections != null) { + // Some connections may alread exist on the node + // if the node has been recycled. We may want to + // keep existing connections to other graphs. + connections.AddRange(node.connections); + } + + int av = node.GetVertexCount(); + + for (int a = 0; a < av; a++) { + int first = node.GetVertexIndex(a); + int second = node.GetVertexIndex((a+1) % av); + + if (nodeRefs.TryGetValue(new Int2(second, first), out var connNode)) { + TriangleMeshNode other = nodes[connNode]; + + int bv = other.GetVertexCount(); + + for (int b = 0; b < bv; b++) { + /// TODO: This will fail on edges which are only partially shared + if (other.GetVertexIndex(b) == second && other.GetVertexIndex((b+1) % bv) == first) { + connections.Add(new Connection( + other, + (uint)(node.position - other.position).costMagnitude, + Connection.PackShapeEdgeInfo((byte)a, (byte)b, true, true, true) + )); + break; + } + } + } + } + + node.connections = connections.ToArrayFromPool(); + node.SetConnectivityDirty(); + } + + nodeRefs.Clear(); + ObjectPoolSimple >.Release(ref nodeRefs); + ListPool.Release(ref connections); + } + + /// + /// Generate connections between the two tiles. + /// The tiles must be adjacent. + /// + internal static void ConnectTiles (NavmeshTile tile1, NavmeshTile tile2, float tileWorldSizeX, float tileWorldSizeZ, float maxTileConnectionEdgeDistance) { + if (tile1 == null || tile2 == null) return; + + if (tile1.nodes == null) throw new System.ArgumentException("tile1 does not contain any nodes"); + if (tile2.nodes == null) throw new System.ArgumentException("tile2 does not contain any nodes"); + + int t1x = Mathf.Clamp(tile2.x, tile1.x, tile1.x+tile1.w-1); + int t2x = Mathf.Clamp(tile1.x, tile2.x, tile2.x+tile2.w-1); + int t1z = Mathf.Clamp(tile2.z, tile1.z, tile1.z+tile1.d-1); + int t2z = Mathf.Clamp(tile1.z, tile2.z, tile2.z+tile2.d-1); + + int coord, altcoord; + int t1coord, t2coord; + + float tileWorldSize; + + // Figure out which side that is shared between the two tiles + // and what coordinate index is fixed along that edge (x or z) + if (t1x == t2x) { + coord = 2; + altcoord = 0; + t1coord = t1z; + t2coord = t2z; + tileWorldSize = tileWorldSizeZ; + } else if (t1z == t2z) { + coord = 0; + altcoord = 2; + t1coord = t1x; + t2coord = t2x; + tileWorldSize = tileWorldSizeX; + } else { + throw new System.ArgumentException("Tiles are not adjacent (neither x or z coordinates match)"); + } + + if (Math.Abs(t1coord-t2coord) != 1) { + throw new System.ArgumentException("Tiles are not adjacent (tile coordinates must differ by exactly 1. Got '" + t1coord + "' and '" + t2coord + "')"); + } + + // Midpoint between the two tiles + int midpoint = (int)Math.Round((Math.Max(t1coord, t2coord) * tileWorldSize) * Int3.Precision); + +#if ASTARDEBUG + Vector3 v1 = new Vector3(-100, 0, -100); + Vector3 v2 = new Vector3(100, 0, 100); + v1[coord] = midpoint*Int3.PrecisionFactor; + v2[coord] = midpoint*Int3.PrecisionFactor; + + Debug.DrawLine(v1, v2, Color.magenta); +#endif + + TriangleMeshNode[] nodes1 = tile1.nodes; + TriangleMeshNode[] nodes2 = tile2.nodes; + + // Find all nodes of the second tile which are adjacent to the border between the tiles. + // This is used to speed up the matching process (the impact can be very significant for large tiles, but is insignificant for small ones). + TriangleMeshNode[] closeToEdge = ArrayPool.Claim(nodes2.Length); + int numCloseToEdge = 0; + for (int j = 0; j < nodes2.Length; j++) { + TriangleMeshNode nodeB = nodes2[j]; + int bVertexCount = nodeB.GetVertexCount(); + for (int b = 0; b < bVertexCount; b++) { + // Note that we cannot use nodeB.GetVertexInGraphSpace because it might be the case that no graph even has this tile yet (common during updates/scanning the graph). + // The node.GetVertexInGraphSpace will try to look up the graph it is contained in. + // So we need to call NavmeshTile.GetVertexInGraphSpace instead. + Int3 bVertex1 = tile2.GetVertexInGraphSpace(nodeB.GetVertexIndex(b)); + Int3 bVertex2 = tile2.GetVertexInGraphSpace(nodeB.GetVertexIndex((b+1) % bVertexCount)); + if (Math.Abs(bVertex1[coord] - midpoint) < 2 && Math.Abs(bVertex2[coord] - midpoint) < 2) { + closeToEdge[numCloseToEdge] = nodes2[j]; + numCloseToEdge++; + break; + } + } + } + + + // Find adjacent nodes on the border between the tiles + for (int i = 0; i < nodes1.Length; i++) { + TriangleMeshNode nodeA = nodes1[i]; + int aVertexCount = nodeA.GetVertexCount(); + + // Loop through all *sides* of the node + for (int a = 0; a < aVertexCount; a++) { + // Vertices that the segment consists of + Int3 aVertex1 = tile1.GetVertexInGraphSpace(nodeA.GetVertexIndex(a)); + Int3 aVertex2 = tile1.GetVertexInGraphSpace(nodeA.GetVertexIndex((a+1) % aVertexCount)); + + // Check if it is really close to the tile border + if (Math.Abs(aVertex1[coord] - midpoint) < 2 && Math.Abs(aVertex2[coord] - midpoint) < 2) { + int minalt = Math.Min(aVertex1[altcoord], aVertex2[altcoord]); + int maxalt = Math.Max(aVertex1[altcoord], aVertex2[altcoord]); + + // Degenerate edge + if (minalt == maxalt) continue; + + for (int j = 0; j < numCloseToEdge; j++) { + TriangleMeshNode nodeB = closeToEdge[j]; + int bVertexCount = nodeB.GetVertexCount(); + for (int b = 0; b < bVertexCount; b++) { + Int3 bVertex1 = tile2.GetVertexInGraphSpace(nodeB.GetVertexIndex(b)); + Int3 bVertex2 = tile2.GetVertexInGraphSpace(nodeB.GetVertexIndex((b+1) % bVertexCount)); + if (Math.Abs(bVertex1[coord] - midpoint) < 2 && Math.Abs(bVertex2[coord] - midpoint) < 2) { + int minalt2 = Math.Min(bVertex1[altcoord], bVertex2[altcoord]); + int maxalt2 = Math.Max(bVertex1[altcoord], bVertex2[altcoord]); + + // Degenerate edge + if (minalt2 == maxalt2) continue; + + if (maxalt > minalt2 && minalt < maxalt2) { + // The two nodes seem to be adjacent + + // Test shortest distance between the segments (first test if they are equal since that is much faster and pretty common) + bool identical = (aVertex1 == bVertex1 && aVertex2 == bVertex2) || (aVertex1 == bVertex2 && aVertex2 == bVertex1); + if (identical || + VectorMath.SqrDistanceSegmentSegment((Vector3)aVertex1, (Vector3)aVertex2, (Vector3)bVertex1, (Vector3)bVertex2) < maxTileConnectionEdgeDistance*maxTileConnectionEdgeDistance) { + uint cost = (uint)(nodeA.position - nodeB.position).costMagnitude; + + nodeA.AddPartialConnection(nodeB, cost, Connection.PackShapeEdgeInfo((byte)a, (byte)b, identical, true, true)); + nodeB.AddPartialConnection(nodeA, cost, Connection.PackShapeEdgeInfo((byte)b, (byte)a, identical, true, true)); + } + } + } + } + } + } + } + } + + ArrayPool.Release(ref closeToEdge); + } + + /// + /// Start batch updating of tiles. + /// During batch updating, tiles will not be connected if they are updating with ReplaceTile. + /// When ending batching, all affected tiles will be connected. + /// This is faster than not using batching. + /// + public void StartBatchTileUpdate () { + if (batchTileUpdate) throw new System.InvalidOperationException("Calling StartBatchLoad when batching is already enabled"); + batchTileUpdate = true; + } + + /// + /// Destroy several nodes simultaneously. + /// This is faster than simply looping through the nodes and calling the node.Destroy method because some optimizations + /// relating to how connections are removed can be optimized. + /// + static void DestroyNodes (List nodes) { + for (int i = 0; i < nodes.Count; i++) { + nodes[i].TemporaryFlag1 = true; + } + + for (int i = 0; i < nodes.Count; i++) { + var node = nodes[i]; + if (node.connections != null) { + for (int j = 0; j < node.connections.Length; j++) { + var neighbour = node.connections[j].node; + if (!neighbour.TemporaryFlag1) { + neighbour.RemovePartialConnection(node); + } + } + + // Remove the connections array explicitly for performance. + // Otherwise the Destroy method will try to remove the connections in both directions one by one which is slow. + ArrayPool.Release(ref node.connections, true); + } + node.Destroy(); + } + } + + void TryConnect (int tileIdx1, int tileIdx2) { + // If both tiles were flagged, then only connect if tileIdx1 < tileIdx2 to make sure we don't connect the tiles twice + // as this method will be called with swapped arguments as well. + if (tiles[tileIdx1].flag && tiles[tileIdx2].flag && tileIdx1 >= tileIdx2) return; + ConnectTiles(tiles[tileIdx1], tiles[tileIdx2], TileWorldSizeX, TileWorldSizeZ, MaxTileConnectionEdgeDistance); + } + + /// + /// End batch updating of tiles. + /// During batch updating, tiles will not be connected if they are updated with ReplaceTile. + /// When ending batching, all affected tiles will be connected. + /// This is faster than not using batching. + /// + public void EndBatchTileUpdate () { + if (!batchTileUpdate) throw new System.InvalidOperationException("Calling EndBatchTileUpdate when batching had not yet been started"); + + batchTileUpdate = false; + + DestroyNodes(batchNodesToDestroy); + batchNodesToDestroy.ClearFast(); + + if (batchUpdatedTiles.Count == 0) return; + for (int i = 0; i < batchUpdatedTiles.Count; i++) tiles[batchUpdatedTiles[i]].flag = true; + + IntRect tileRect = default; + for (int i = 0; i < batchUpdatedTiles.Count; i++) { + int x = batchUpdatedTiles[i] % tileXCount, z = batchUpdatedTiles[i] / tileXCount; + if (i == 0) tileRect = new IntRect(x, z, x, z); + else tileRect = tileRect.ExpandToContain(x, z); + + if (x > 0) TryConnect(batchUpdatedTiles[i], batchUpdatedTiles[i] - 1); + if (x < tileXCount - 1) TryConnect(batchUpdatedTiles[i], batchUpdatedTiles[i] + 1); + if (z > 0) TryConnect(batchUpdatedTiles[i], batchUpdatedTiles[i] - tileXCount); + if (z < tileZCount - 1) TryConnect(batchUpdatedTiles[i], batchUpdatedTiles[i] + tileXCount); + } + + for (int i = 0; i < batchUpdatedTiles.Count; i++) tiles[batchUpdatedTiles[i]].flag = false; + batchUpdatedTiles.ClearFast(); + DirtyBounds(GetTileBounds(tileRect)); + } + + /// Clears the tiles in the specified rectangle. + /// The rectangle in tile coordinates to clear. The coordinates are in tile coordinates, not world coordinates. + public void ClearTiles (IntRect tileRect) { + AssertSafeToUpdateGraph(); + var wasBatching = batchTileUpdate; + if (!wasBatching) StartBatchTileUpdate(); + var graphTileRect = new IntRect(0, 0, tileXCount-1, tileZCount-1); + tileRect = IntRect.Intersection(tileRect, graphTileRect); + + for (int z = tileRect.ymin; z <= tileRect.ymax; z++) { + for (int x = tileRect.xmin; x <= tileRect.xmax; x++) { + ClearTile(x, z); + } + } + if (!wasBatching) EndBatchTileUpdate(); + } + + /// + /// Clear the tile at the specified coordinate. + /// Must be called during a batch update, see . + /// + protected void ClearTile (int x, int z) { + if (!batchTileUpdate) throw new System.Exception("Must be called during a batch update. See StartBatchTileUpdate"); + var tile = GetTile(x, z); + if (tile == null) return; + var nodes = tile.nodes; + for (int i = 0; i < nodes.Length; i++) { + if (nodes[i] != null) batchNodesToDestroy.Add(nodes[i]); + } + tile.Dispose(); + tiles[x + z*tileXCount] = null; + } + + /// Temporary buffer used in + Dictionary nodeRecyclingHashBuffer = new Dictionary(); + + /// + /// Reuse nodes that keep the exact same vertices after a tile replacement. + /// The reused nodes will be added to the recycledNodeBuffer array at the index corresponding to the + /// indices in the triangle array that its vertices uses. + /// + /// All connections on the reused nodes will be removed except ones that go to other graphs. + /// The reused nodes will be removed from the tile by replacing it with a null slot in the node array. + /// + /// See: + /// + void PrepareNodeRecycling (int x, int z, UnsafeSpan verts, UnsafeSpan tris, TriangleMeshNode[] recycledNodeBuffer) { + NavmeshTile tile = GetTile(x, z); + + if (tile == null || tile.nodes.Length == 0) return; + var nodes = tile.nodes; + var recycling = nodeRecyclingHashBuffer; + const int P1 = 31; + const int P2 = 196613; + const int P3 = 3145739; + for (int i = 0, j = 0; i < tris.Length; i += 3, j++) { + recycling[(verts[tris[i+0]].GetHashCode()*P1) ^ (verts[tris[i+1]].GetHashCode()*P2) ^ (verts[tris[i+2]].GetHashCode()*P3)] = j; + } + var connectionsToKeep = ListPool.Claim(); + + for (int i = 0; i < nodes.Length; i++) { + var node = nodes[i]; + node.GetVerticesInGraphSpace(out var v0, out var v1, out var v2); + var hash = (v0.GetHashCode()*P1) ^ (v1.GetHashCode()*P2) ^ (v2.GetHashCode()*P3); + if (recycling.TryGetValue(hash, out int newNodeIndex)) { + // Technically we should check for a cyclic permutations of the vertices (e.g node a,b,c could become node b,c,a) + // but in almost all cases the vertices will keep the same order. Allocating one or two extra nodes isn't such a big deal. + if (verts[tris[3*newNodeIndex+0]] == v0 && verts[tris[3*newNodeIndex+1]] == v1 && verts[tris[3*newNodeIndex+2]] == v2) { + recycledNodeBuffer[newNodeIndex] = node; + // Remove the node from the tile + nodes[i] = null; + // Only keep connections to nodes on other graphs + // Usually there are no connections to nodes to other graphs and this is faster than removing all connections them one by one + if (node.connections != null) { + for (int j = 0; j < node.connections.Length; j++) { + if (node.connections[j].node.GraphIndex != node.GraphIndex) { + connectionsToKeep.Add(node.connections[j]); + } + } + ArrayPool.Release(ref node.connections, true); + } + if (connectionsToKeep.Count > 0) { + node.connections = connectionsToKeep.ToArrayFromPool(); + node.SetConnectivityDirty(); + connectionsToKeep.Clear(); + } + } + } + } + + recycling.Clear(); + ListPool.Release(ref connectionsToKeep); + } + + /// + /// Replace tile at index with nodes created from specified navmesh. + /// This will create new nodes and link them to the adjacent tile (unless batching has been started in which case that will be done when batching ends). + /// + /// See: + /// + /// X coordinate of the tile to replace. + /// Z coordinate of the tile to replace. + /// Vertices of the new tile. The vertices are assumed to be in 'tile space', that is being in a rectangle with one corner at the origin and one at (#TileWorldSizeX, 0, #TileWorldSizeZ). + /// Triangles of the new tile. If #RecalculateNormals is enabled, the triangles will be converted to clockwise order (when seen from above), if they are not already. + /// Tags for the nodes. The array must have the same length as the tris array divided by 3. If null, the tag will be set to 0 for all nodes. + /// If true, existing tags and penalties will be preserved for nodes that stay in exactly the same position after the tile replacement. + public void ReplaceTile (int x, int z, Int3[] verts, int[] tris, uint[] tags = null, bool tryPreserveExistingTagsAndPenalties = true) { + AssertSafeToUpdateGraph(); + int w = 1, d = 1; + + if (x + w > tileXCount || z+d > tileZCount || x < 0 || z < 0) { + throw new System.ArgumentException("Tile is placed at an out of bounds position or extends out of the graph bounds ("+x+", " + z + " [" + w + ", " + d+ "] " + tileXCount + " " + tileZCount + ")"); + } + + if (tris.Length % 3 != 0) throw new System.ArgumentException("Triangle array's length must be a multiple of 3 (tris)"); + if (tags != null && tags.Length != tris.Length / 3) throw new System.ArgumentException("Triangle array must be 3 times the size of the tags array"); + if (verts.Length > VertexIndexMask) { + Debug.LogError("Too many vertices in the tile (" + verts.Length + " > " + VertexIndexMask +")\nYou can enable ASTAR_RECAST_LARGER_TILES under the 'Optimizations' tab in the A* Inspector to raise this limit. Or you can use a smaller tile size to reduce the likelihood of this happening."); + verts = new Int3[0]; + tris = new int[0]; + } + + var wasNotBatching = !batchTileUpdate; + if (wasNotBatching) StartBatchTileUpdate(); + Profiler.BeginSample("Tile Initialization"); + var trisSpan = new UnsafeSpan(Allocator.Persistent, tris.Length); + trisSpan.CopyFrom(tris); + var vertsInGraphSpace = new UnsafeSpan(Allocator.Persistent, verts.Length); + vertsInGraphSpace.CopyFrom(verts); + + var offset = (Int3) new Vector3(x * TileWorldSizeX, 0, z * TileWorldSizeZ); + for (int i = 0; i < verts.Length; i++) { + vertsInGraphSpace[i] += offset; + } + + var vertsInWorldSpace = vertsInGraphSpace.Clone(Allocator.Persistent); + transform.Transform(vertsInWorldSpace); + + Profiler.BeginSample("AABBTree Rebuild"); + var bbTree = new BBTree(trisSpan, vertsInGraphSpace); + Profiler.EndSample(); + + // Create a new navmesh tile and assign its settings + var tile = new NavmeshTile { + x = x, + z = z, + w = w, + d = d, + tris = trisSpan, + vertsInGraphSpace = vertsInGraphSpace, + verts = vertsInWorldSpace, + bbTree = bbTree, + graph = this, + }; + + if (!Mathf.Approximately(x*TileWorldSizeX*Int3.FloatPrecision, (float)Math.Round(x*TileWorldSizeX*Int3.FloatPrecision))) Debug.LogWarning("Possible numerical imprecision. Consider adjusting tileSize and/or cellSize"); + if (!Mathf.Approximately(z*TileWorldSizeZ*Int3.FloatPrecision, (float)Math.Round(z*TileWorldSizeZ*Int3.FloatPrecision))) Debug.LogWarning("Possible numerical imprecision. Consider adjusting tileSize and/or cellSize"); + + Profiler.BeginSample("Clear Previous Tiles"); + + // Create a backing array for the new nodes + tile.nodes = new TriangleMeshNode[trisSpan.Length/3]; + // Recycle any nodes that are in the exact same spot after replacing the tile. + // This also keeps e.g penalties and tags and other connections which might be useful. + // It also avoids trashing the paths for the RichAI component (as it will have to immediately recalculate its path + // if it discovers that its path contains destroyed nodes). + PrepareNodeRecycling(x, z, vertsInGraphSpace, trisSpan, tile.nodes); + // Remove previous tiles (except the nodes that were recycled above) + ClearTile(x, z); + + Profiler.EndSample(); + Profiler.EndSample(); + + Profiler.BeginSample("Assign Node Data"); + + // Set tile + tiles[x + z*tileXCount] = tile; + batchUpdatedTiles.Add(x + z*tileXCount); + + if (RecalculateNormals) MeshUtility.MakeTrianglesClockwise(ref tile.vertsInGraphSpace, ref tile.tris); + + // Create nodes and assign triangle indices + ulong gcHandle = 0; + var tagsSpan = tags != null ? new UnsafeSpan(tags, out gcHandle) : default; + CreateNodes(tile, tile.tris, x + z*tileXCount, (uint)active.data.GetGraphIndex(this), tagsSpan, true, active, initialPenalty, tryPreserveExistingTagsAndPenalties); + if (tags != null) Unity.Collections.LowLevel.Unsafe.UnsafeUtility.ReleaseGCObject(gcHandle); + + Profiler.EndSample(); + + Profiler.BeginSample("Create Node Connections"); + CreateNodeConnections(tile.nodes, keepExistingConnections: true); + Profiler.EndSample(); + + Profiler.BeginSample("Connect With Neighbours"); + + if (wasNotBatching) EndBatchTileUpdate(); + Profiler.EndSample(); + } + + internal static void CreateNodes (NavmeshTile tile, UnsafeSpan tris, int tileIndex, uint graphIndex, UnsafeSpan tags, bool initializeNodes, AstarPath astar, uint initialPenalty, bool tryPreserveExistingTagsAndPenalties) { + var nodes = tile.nodes; + + if (nodes == null || nodes.Length < tris.Length/3) throw new System.ArgumentException("nodes must be non null and at least as large as tris.Length/3"); + // This index will be ORed to the triangle indices + tileIndex <<= TileIndexOffset; + + // Create nodes and assign vertex indices + for (int i = 0; i < nodes.Length; i++) { + var node = nodes[i]; + bool newNode = false; + // Allow the nodes to be partially filled in already to allow for recycling nodes + if (node == null) { + newNode = true; + if (initializeNodes) { + node = nodes[i] = new TriangleMeshNode(astar); + } else { + // We create nodes in a destroyed state to avoid the nodes being added to the global node list. + // This allows us to create the nodes in a separate thread without causing race conditions. + node = nodes[i] = new TriangleMeshNode(); + } + } + + // If tryPreserveExistingTagsAndPenalties is true, we fill in the tag and penalty only if the node wasn't recycled + if (!tryPreserveExistingTagsAndPenalties || newNode) { + if (tags.Length > 0) { + node.Tag = tags[i]; + } + node.Penalty = initialPenalty; + } + + // Reset all relevant fields on the node (even on recycled nodes to avoid exposing internal implementation details) + node.Walkable = true; + node.GraphIndex = graphIndex; + // The vertices stored on the node are composed + // out of the triangle index and the tile index + node.v0 = tris[i*3+0] | tileIndex; + node.v1 = tris[i*3+1] | tileIndex; + node.v2 = tris[i*3+2] | tileIndex; + + // This is equivalent to calling node.UpdatePositionFromVertices(), but that would require the tile to be attached to a graph, which it might not be at this stage. + node.position = (tile.GetVertex(node.v0) + tile.GetVertex(node.v1) + tile.GetVertex(node.v2)) * (1.0f/3.0f); + } + } + + public NavmeshBase () { + navmeshUpdateData = new NavmeshUpdates.NavmeshUpdateSettings(this); + } + + /// + /// Returns if there is an obstacle between start and end on the graph. + /// This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection. + /// + /// [Open online documentation to see images] + /// + public bool Linecast (Vector3 start, Vector3 end) { + return Linecast(start, end, null); + } + + /// + /// Returns if there is an obstacle between start and end on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection. + /// + /// [Open online documentation to see images] + /// + /// Point to linecast from. + /// Point to linecast to. + /// Contains info on what was hit, see GraphHitInfo. + /// If you know which node the start point is on, you can pass it here to save a GetNearest call, resulting in a minor performance boost. Otherwise, pass null. The start point will be clamped to the surface of this node. + public bool Linecast (Vector3 start, Vector3 end, GraphNode hint, out GraphHitInfo hit) { + return Linecast(this, start, end, hint, out hit, null); + } + + /// + /// Returns if there is an obstacle between start and end on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection. + /// + /// [Open online documentation to see images] + /// + /// Point to linecast from. + /// Point to linecast to. + /// If you know which node the start point is on, you can pass it here to save a GetNearest call, resulting in a minor performance boost. Otherwise, pass null. The start point will be clamped to the surface of this node. + public bool Linecast (Vector3 start, Vector3 end, GraphNode hint) { + GraphHitInfo hit; + + return Linecast(this, start, end, hint, out hit, null); + } + + /// + /// Returns if there is an obstacle between start and end on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection. + /// + /// [Open online documentation to see images] + /// + /// Point to linecast from. + /// Point to linecast to. + /// Contains info on what was hit, see GraphHitInfo. + /// If a list is passed, then it will be filled with all nodes the linecast traverses. + /// If you know which node the start point is on, you can pass it here to save a GetNearest call, resulting in a minor performance boost. Otherwise, pass null. The start point will be clamped to the surface of this node. + public bool Linecast (Vector3 start, Vector3 end, GraphNode hint, out GraphHitInfo hit, List trace) { + return Linecast(this, start, end, hint, out hit, trace); + } + + /// + /// Returns if there is an obstacle between start and end on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection. + /// + /// [Open online documentation to see images] + /// + /// Point to linecast from. + /// Point to linecast to. + /// Contains info on what was hit, see GraphHitInfo. + /// If a list is passed, then it will be filled with all nodes the linecast traverses. + /// If not null then the delegate will be called for each node and if it returns false the node will be treated as unwalkable and a hit will be returned. + /// Note that unwalkable nodes are always treated as unwalkable regardless of what this filter returns. + public bool Linecast (Vector3 start, Vector3 end, out GraphHitInfo hit, List trace, System.Func filter) { + return Linecast(this, start, end, null, out hit, trace, filter); + } + + /// + /// Returns if there is an obstacle between start and end on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection. + /// + /// [Open online documentation to see images] + /// + /// Point to linecast from. + /// Point to linecast to. + /// Contains info on what was hit, see GraphHitInfo. + /// If a list is passed, then it will be filled with all nodes the linecast traverses. + /// If not null then the delegate will be called for each node and if it returns false the node will be treated as unwalkable and a hit will be returned. + /// Note that unwalkable nodes are always treated as unwalkable regardless of what this filter returns. + /// If you know which node the start point is on, you can pass it here to save a GetNearest call, resulting in a minor performance boost. Otherwise, pass null. The start point will be clamped to the surface of this node. + public bool Linecast (Vector3 start, Vector3 end, GraphNode hint, out GraphHitInfo hit, List trace, System.Func filter) { + return Linecast(this, start, end, null, out hit, trace, filter); + } + + + /// + /// Returns if there is an obstacle between start and end on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersection. + /// + /// [Open online documentation to see images] + /// + /// The graph to perform the search on. + /// Point to start from. + /// Point to linecast to. + /// Contains info on what was hit, see GraphHitInfo. + /// If you know which node the start point is on, you can pass it here to save a GetNearest call, resulting in a minor performance boost. Otherwise, pass null. The start point will be clamped to the surface of this node. + public static bool Linecast (NavmeshBase graph, Vector3 start, Vector3 end, GraphNode hint, out GraphHitInfo hit) { + return Linecast(graph, start, end, hint, out hit, null); + } + + /// Cached with distanceXZ=true to reduce allocations + static readonly NNConstraint NNConstraintNoneXZ = new NNConstraint { + constrainWalkability = false, + constrainArea = false, + constrainTags = false, + constrainDistance = false, + graphMask = -1, + }; + + /// Used to optimize linecasts by precomputing some values + static readonly byte[] LinecastShapeEdgeLookup; + + static NavmeshBase () { + // Want want to figure out which side of a triangle that a ray exists using. + // There are only 3*3*3 = 27 different options for the [left/right/colinear] options for the 3 vertices of a triangle. + // So we can precompute the result to improve the performance of linecasts. + // For simplicity we reserve 2 bits for each side which means that we have 4*4*4 = 64 entries in the lookup table. + LinecastShapeEdgeLookup = new byte[64]; + Side[] sideOfLine = new Side[3]; + for (int i = 0; i < LinecastShapeEdgeLookup.Length; i++) { + sideOfLine[0] = (Side)((i >> 0) & 0x3); + sideOfLine[1] = (Side)((i >> 2) & 0x3); + sideOfLine[2] = (Side)((i >> 4) & 0x3); + LinecastShapeEdgeLookup[i] = 0xFF; + // Value 3 is an invalid value. So we just skip it. + if (sideOfLine[0] != (Side)3 && sideOfLine[1] != (Side)3 && sideOfLine[2] != (Side)3) { + // Figure out the side of the triangle that the line exits. + // In case the line passes through one of the vertices of the triangle + // there may be multiple alternatives. In that case pick the edge + // which contains the fewest vertices that lie on the line. + // This prevents a potential infinite loop when a linecast is done colinear + // to the edge of a triangle. + int bestBadness = int.MaxValue; + for (int j = 0; j < 3; j++) { + if ((sideOfLine[j] == Side.Left || sideOfLine[j] == Side.Colinear) && (sideOfLine[(j+1)%3] == Side.Right || sideOfLine[(j+1)%3] == Side.Colinear)) { + var badness = (sideOfLine[j] == Side.Colinear ? 1 : 0) + (sideOfLine[(j+1)%3] == Side.Colinear ? 1 : 0); + if (badness < bestBadness) { + LinecastShapeEdgeLookup[i] = (byte)j; + bestBadness = badness; + } + } + } + } + } + } + + /// + /// Returns if there is an obstacle between origin and end on the graph. + /// + /// This is not the same as Physics.Linecast, this function traverses the \b graph and looks for collisions instead of checking for collider intersections. + /// + /// Note: This method only makes sense for graphs in which there is a definite 'up' direction. For example it does not make sense for e.g spherical graphs, + /// navmeshes in which characters can walk on walls/ceilings or other curved worlds. If you try to use this method on such navmeshes it may output nonsense. + /// + /// [Open online documentation to see images] + /// + /// The graph to perform the search on + /// Point to start from. This point should be on the navmesh. It will be snapped to the closest point on the navmesh otherwise. + /// Point to linecast to + /// Contains info on what was hit, see GraphHitInfo + /// If you already know the node which contains the origin point, you may pass it here for slighly improved performance. If null, a search for the closest node will be done. + /// If a list is passed, then it will be filled with all nodes along the line up until it hits an obstacle or reaches the end. + /// If not null then the delegate will be called for each node and if it returns false the node will be treated as unwalkable and a hit will be returned. + /// Note that unwalkable nodes are always treated as unwalkable regardless of what this filter returns. + public static bool Linecast (NavmeshBase graph, Vector3 origin, Vector3 end, GraphNode hint, out GraphHitInfo hit, List trace, System.Func filter = null) { + if (!graph.RecalculateNormals) { + throw new System.InvalidOperationException("The graph is configured to not recalculate normals. This is typically used for spherical navmeshes or other non-planar ones. Linecasts cannot be done on such navmeshes. Enable 'Recalculate Normals' on the navmesh graph if you want to use linecasts."); + } + + hit = new GraphHitInfo(); + + if (float.IsNaN(origin.x + origin.y + origin.z)) throw new System.ArgumentException("origin is NaN"); + if (float.IsNaN(end.x + end.y + end.z)) throw new System.ArgumentException("end is NaN"); + + var node = hint as TriangleMeshNode; + NNConstraintNoneXZ.distanceMetric = DistanceMetric.ClosestAsSeenFromAbove(); + if (node == null) { + var nn = graph.GetNearest(origin, NNConstraintNoneXZ); + node = nn.node as TriangleMeshNode; + + if (node == null || nn.distanceCostSqr > 0.001f*0.001f) { + hit.origin = origin; + hit.point = origin; + return true; + } + } + + // Snap the origin to the navmesh (particularly important when using a hint) + var i3originInGraphSpace = node.ClosestPointOnNodeXZInGraphSpace(origin); + hit.origin = graph.transform.Transform((Vector3)i3originInGraphSpace); + + if (!node.Walkable || (filter != null && !filter(node))) { + hit.node = node; + hit.point = hit.origin; + hit.tangentOrigin = hit.origin; + return true; + } + + var endInGraphSpace = graph.transform.InverseTransform(end); + var i3endInGraphSpace = (Int3)endInGraphSpace; + + // Fast early out check + if (i3originInGraphSpace == i3endInGraphSpace) { + hit.point = hit.origin; + hit.node = node; + if (trace != null) trace.Add(node); + return false; + } + + int counter = 0; + while (true) { + counter++; + if (counter > 2000) { + Debug.LogError("Linecast was stuck in infinite loop. Breaking."); + return true; + } + + trace?.Add(node); + + node.GetVerticesInGraphSpace(out var a0, out var a1, out var a2); + int sideOfLine = (byte)VectorMath.SideXZ(i3originInGraphSpace, i3endInGraphSpace, a0); + sideOfLine |= (byte)VectorMath.SideXZ(i3originInGraphSpace, i3endInGraphSpace, a1) << 2; + sideOfLine |= (byte)VectorMath.SideXZ(i3originInGraphSpace, i3endInGraphSpace, a2) << 4; + // Use a lookup table to figure out which side of this triangle that the ray exits + int shapeEdgeA = (int)LinecastShapeEdgeLookup[sideOfLine]; + // The edge consists of the vertex with index 'sharedEdgeA' and the next vertex after that (index '(sharedEdgeA+1)%3') + + var sideNodeExit = VectorMath.SideXZ(shapeEdgeA == 0 ? a0 : (shapeEdgeA == 1 ? a1 : a2), shapeEdgeA == 0 ? a1 : (shapeEdgeA == 1 ? a2 : a0), i3endInGraphSpace); + if (sideNodeExit != Side.Left) { + // Ray stops before it leaves the current node. + // The endpoint must be inside the current node. + + hit.point = end; + hit.node = node; + + var endNode = graph.GetNearest(end, NNConstraintNoneXZ).node as TriangleMeshNode; + if (endNode == node || endNode == null) { + // We ended up at the right node. + // If endNode == null we also take this branch. + // That case may happen if a linecast is made to a point, but the point way a very large distance straight up into the air. + // The linecast may indeed reach the right point, but it's so far away up into the air that the GetNearest method will stop searching. + return false; + } else { + // The closest node to the end point was not the node we ended up at. + // This can happen if a linecast is done between two floors of a building. + // The linecast may reach the right location when seen from above + // but it will have ended up on the wrong floor of the building. + // This indicates that the start and end points cannot be connected by a valid straight line on the navmesh. + return true; + } + } + + if (shapeEdgeA == 0xFF) { + // Line does not intersect node at all? + // This may theoretically happen if the origin was not properly snapped to the inside of the triangle, but is instead a tiny distance outside the node. + Debug.LogError("Line does not intersect node at all"); + hit.node = node; + hit.point = hit.tangentOrigin = hit.origin; + return true; + } else { + bool success = false; + var nodeConnections = node.connections; + + // Check all node connetions to see which one is the next node along the ray's path + for (int i = 0; i < nodeConnections.Length; i++) { + if (nodeConnections[i].isEdgeShared && nodeConnections[i].isOutgoing && nodeConnections[i].shapeEdge == shapeEdgeA) { + // This might be the next node that we enter + + var neighbour = nodeConnections[i].node as TriangleMeshNode; + if (neighbour == null || !neighbour.Walkable || (filter != null && !filter(neighbour))) continue; + + int shapeEdgeB = nodeConnections[i].adjacentShapeEdge; + + var side1 = VectorMath.SideXZ(i3originInGraphSpace, i3endInGraphSpace, neighbour.GetVertexInGraphSpace(shapeEdgeB)); + var side2 = VectorMath.SideXZ(i3originInGraphSpace, i3endInGraphSpace, neighbour.GetVertexInGraphSpace((shapeEdgeB+1) % 3)); + + // Check if the line enters this edge + success = (side1 == Side.Right || side1 == Side.Colinear) && (side2 == Side.Left || side2 == Side.Colinear); + + if (!success) continue; + + // Ray has entered the neighbouring node. + // After the first node, it is possible to prove the loop invariant that shapeEdgeA will *never* end up as -1 (checked above) + // Since side = Colinear acts essentially as a wildcard. side1 and side2 can be the most restricted if they are side1=right, side2=left. + // Then when we get to the next node we know that the sideOfLine array is either [*, Right, Left], [Left, *, Right] or [Right, Left, *], where * is unknown. + // We are looking for the sequence [Left, Right] (possibly including Colinear as wildcard). We will always find this sequence regardless of the value of *. + node = neighbour; + break; + } + } + + if (!success) { + // Node did not enter any neighbours + // It must have hit the border of the navmesh + var hitEdgeStartInGraphSpace = (Vector3)(shapeEdgeA == 0 ? a0 : (shapeEdgeA == 1 ? a1 : a2)); + var hitEdgeEndInGraphSpace = (Vector3)(shapeEdgeA == 0 ? a1 : (shapeEdgeA == 1 ? a2 : a0)); + var intersectionInGraphSpace = VectorMath.LineIntersectionPointXZ(hitEdgeStartInGraphSpace, hitEdgeEndInGraphSpace, (Vector3)i3originInGraphSpace, (Vector3)i3endInGraphSpace); + hit.point = graph.transform.Transform(intersectionInGraphSpace); + hit.node = node; + var hitEdgeStart = graph.transform.Transform(hitEdgeStartInGraphSpace); + var hitEdgeEnd = graph.transform.Transform(hitEdgeEndInGraphSpace); + hit.tangent = hitEdgeEnd - hitEdgeStart; + hit.tangentOrigin = hitEdgeStart; + return true; + } + } + } + } + + public override void OnDrawGizmos (DrawingData gizmos, bool drawNodes, RedrawScope redrawScope) { + if (!drawNodes) { + return; + } + + using (var builder = gizmos.GetBuilder(redrawScope)) { + var bounds = new Bounds(); + bounds.SetMinMax(Vector3.zero, forcedBoundsSize); + // Draw a write cube using the latest transform + // (this makes the bounds update immediately if some field is changed in the editor) + using (builder.WithMatrix(CalculateTransform().matrix)) { + builder.WireBox(bounds, Color.white); + } + } + + if (tiles != null && (showMeshSurface || showMeshOutline || showNodeConnections)) { + var baseHasher = new NodeHasher(active); + baseHasher.Add(showMeshOutline ? 1 : 0); + baseHasher.Add(showMeshSurface ? 1 : 0); + baseHasher.Add(showNodeConnections ? 1 : 0); + + int startTileIndex = 0; + var hasher = baseHasher; + var hashedNodes = 0; + + // Update navmesh vizualizations for + // the tiles that have been changed + for (int i = 0; i < tiles.Length; i++) { + // This may happen if an exception has been thrown when the graph was scanned. + // We don't want the gizmo code to start to throw exceptions as well then as + // that would obscure the actual source of the error. + if (tiles[i] == null) continue; + + // Calculate a hash of the tile + var nodes = tiles[i].nodes; + for (int j = 0; j < nodes.Length; j++) { + hasher.HashNode(nodes[j]); + } + hashedNodes += nodes.Length; + + // Note: do not batch more than some large number of nodes at a time. + // Also do not batch more than a single "row" of the graph at once + // because otherwise a small change in one part of the graph could invalidate + // the caches almost everywhere else. + // When restricting the caches to row by row a change in a row + // will never invalidate the cache in another row. + if (hashedNodes > 1024 || (i % tileXCount) == tileXCount - 1 || i == tiles.Length - 1) { + if (!gizmos.Draw(hasher, redrawScope)) { + using (var helper = GraphGizmoHelper.GetGizmoHelper(gizmos, active, hasher, redrawScope)) { + if (showMeshSurface || showMeshOutline) { + CreateNavmeshSurfaceVisualization(tiles, startTileIndex, i + 1, helper); + CreateNavmeshOutlineVisualization(tiles, startTileIndex, i + 1, helper); + } + + if (showNodeConnections) { + for (int ti = startTileIndex; ti <= i; ti++) { + if (tiles[ti] == null) continue; + + var tileNodes = tiles[ti].nodes; + for (int j = 0; j < tileNodes.Length; j++) { + helper.DrawConnections(tileNodes[j]); + } + } + } + } + } + + startTileIndex = i + 1; + hasher = baseHasher; + hashedNodes = 0; + } + } + } + + if (active.showUnwalkableNodes) DrawUnwalkableNodes(gizmos, active.unwalkableNodeDebugSize, redrawScope); + } + + /// Creates a mesh of the surfaces of the navmesh for use in OnDrawGizmos in the editor + void CreateNavmeshSurfaceVisualization (NavmeshTile[] tiles, int startTile, int endTile, GraphGizmoHelper helper) { + int numNodes = 0; + + for (int i = startTile; i < endTile; i++) if (tiles[i] != null) numNodes += tiles[i].nodes.Length; + + // Vertex array might be a bit larger than necessary, but that's ok + var vertices = ArrayPool.Claim(numNodes*3); + var colors = ArrayPool.Claim(numNodes*3); + int offset = 0; + for (int i = startTile; i < endTile; i++) { + var tile = tiles[i]; + if (tile == null) continue; + + for (int j = 0; j < tile.nodes.Length; j++) { + var node = tile.nodes[j]; + Int3 v0, v1, v2; + node.GetVertices(out v0, out v1, out v2); + int index = offset + j*3; + vertices[index + 0] = (Vector3)v0; + vertices[index + 1] = (Vector3)v1; + vertices[index + 2] = (Vector3)v2; + + var color = helper.NodeColor(node); + colors[index + 0] = colors[index + 1] = colors[index + 2] = color; + } + offset += tile.nodes.Length * 3; + } + + if (showMeshSurface) helper.DrawTriangles(vertices, colors, numNodes); + if (showMeshOutline) helper.DrawWireTriangles(vertices, colors, numNodes); + + // Return lists to the pool + ArrayPool.Release(ref vertices); + ArrayPool.Release(ref colors); + } + + /// Creates an outline of the navmesh for use in OnDrawGizmos in the editor + static void CreateNavmeshOutlineVisualization (NavmeshTile[] tiles, int startTile, int endTile, GraphGizmoHelper helper) { + var sharedEdges = new bool[3]; + + for (int i = startTile; i < endTile; i++) { + var tile = tiles[i]; + if (tile == null) continue; + + for (int j = 0; j < tile.nodes.Length; j++) { + sharedEdges[0] = sharedEdges[1] = sharedEdges[2] = false; + + var node = tile.nodes[j]; + if (node.connections != null) { + for (int c = 0; c < node.connections.Length; c++) { + var other = node.connections[c].node as TriangleMeshNode; + + // Loop through neighbours to figure out which edges are shared + if (other != null && other.GraphIndex == node.GraphIndex) { + for (int v = 0; v < 3; v++) { + for (int v2 = 0; v2 < 3; v2++) { + if (node.GetVertexIndex(v) == other.GetVertexIndex((v2+1)%3) && node.GetVertexIndex((v+1)%3) == other.GetVertexIndex(v2)) { + // Found a shared edge with the other node + sharedEdges[v] = true; + v = 3; + break; + } + } + } + } + } + } + + var color = helper.NodeColor(node); + for (int v = 0; v < 3; v++) { + if (!sharedEdges[v]) { + helper.builder.Line((Vector3)node.GetVertex(v), (Vector3)node.GetVertex((v+1)%3), color); + } + } + } + } + } + + /// + /// Serializes Node Info. + /// Should serialize: + /// - Base + /// - Node Flags + /// - Node Penalties + /// - Node + /// - Node Positions (if applicable) + /// - Any other information necessary to load the graph in-game + /// All settings marked with json attributes (e.g JsonMember) have already been + /// saved as graph settings and do not need to be handled here. + /// + /// It is not necessary for this implementation to be forward or backwards compatible. + /// + protected override void SerializeExtraInfo (GraphSerializationContext ctx) { + BinaryWriter writer = ctx.writer; + + if (tiles == null) { + writer.Write(-1); + return; + } + writer.Write(tileXCount); + writer.Write(tileZCount); + + for (int z = 0; z < tileZCount; z++) { + for (int x = 0; x < tileXCount; x++) { + NavmeshTile tile = tiles[x + z*tileXCount]; + + if (tile == null) throw new System.NullReferenceException(); + + writer.Write(tile.x); + writer.Write(tile.z); + + if (tile.x != x || tile.z != z) continue; + + writer.Write(tile.w); + writer.Write(tile.d); + + writer.Write(tile.tris.Length); + + for (int i = 0; i < tile.tris.Length; i++) writer.Write(tile.tris[i]); + + writer.Write(tile.verts.Length); + for (int i = 0; i < tile.verts.Length; i++) { + ctx.SerializeInt3(tile.verts[i]); + } + + writer.Write(tile.vertsInGraphSpace.Length); + for (int i = 0; i < tile.vertsInGraphSpace.Length; i++) { + ctx.SerializeInt3(tile.vertsInGraphSpace[i]); + } + + writer.Write(tile.nodes.Length); + for (int i = 0; i < tile.nodes.Length; i++) { + tile.nodes[i].SerializeNode(ctx); + } + } + } + } + + protected override void DeserializeExtraInfo (GraphSerializationContext ctx) { + BinaryReader reader = ctx.reader; + + tileXCount = reader.ReadInt32(); + + if (tileXCount < 0) return; + + tileZCount = reader.ReadInt32(); + transform = CalculateTransform(); + + Assert.IsNull(tiles); + tiles = new NavmeshTile[tileXCount * tileZCount]; + + // Make sure mesh nodes can reference this graph + TriangleMeshNode.SetNavmeshHolder((int)ctx.graphIndex, this); + + for (int z = 0; z < tileZCount; z++) { + for (int x = 0; x < tileXCount; x++) { + int tileIndex = x + z*tileXCount; + int tx = reader.ReadInt32(); + if (tx < 0) throw new System.Exception("Invalid tile coordinates (x < 0)"); + + int tz = reader.ReadInt32(); + if (tz < 0) throw new System.Exception("Invalid tile coordinates (z < 0)"); + + // This is not the origin of a large tile. Refer back to that tile. + if (tx != x || tz != z) { + tiles[tileIndex] = tiles[tz*tileXCount + tx]; + continue; + } + + var tile = tiles[tileIndex] = new NavmeshTile { + x = tx, + z = tz, + w = reader.ReadInt32(), + d = reader.ReadInt32(), + bbTree = default, + graph = this, + }; + + int trisCount = reader.ReadInt32(); + + if (trisCount % 3 != 0) throw new System.Exception("Corrupt data. Triangle indices count must be divisable by 3. Read " + trisCount); + + tile.tris = new UnsafeSpan(Allocator.Persistent, trisCount); + for (int i = 0; i < tile.tris.Length; i++) tile.tris[i] = reader.ReadInt32(); + + tile.verts = new UnsafeSpan(Allocator.Persistent, reader.ReadInt32()); + for (int i = 0; i < tile.verts.Length; i++) { + tile.verts[i] = ctx.DeserializeInt3(); + } + + if (ctx.meta.version.Major >= 4) { + tile.vertsInGraphSpace = new UnsafeSpan(Allocator.Persistent, reader.ReadInt32()); + if (tile.vertsInGraphSpace.Length != tile.verts.Length) throw new System.Exception("Corrupt data. Array lengths did not match"); + for (int i = 0; i < tile.verts.Length; i++) { + tile.vertsInGraphSpace[i] = ctx.DeserializeInt3(); + } + } else { + // Compatibility + tile.vertsInGraphSpace = new UnsafeSpan(Allocator.Persistent, tile.verts.Length); + tile.verts.CopyTo(tile.vertsInGraphSpace); + transform.InverseTransform(tile.vertsInGraphSpace); + } + + int nodeCount = reader.ReadInt32(); + tile.nodes = new TriangleMeshNode[nodeCount]; + + // Prepare for storing in vertex indices + tileIndex <<= TileIndexOffset; + + for (int i = 0; i < tile.nodes.Length; i++) { + var node = new TriangleMeshNode(active); + tile.nodes[i] = node; + + node.DeserializeNode(ctx); + + node.v0 = tile.tris[i*3+0] | tileIndex; + node.v1 = tile.tris[i*3+1] | tileIndex; + node.v2 = tile.tris[i*3+2] | tileIndex; + node.UpdatePositionFromVertices(); + } + + tile.bbTree = new BBTree(tile.tris, tile.vertsInGraphSpace); + } + } + } + + protected override void PostDeserialization (GraphSerializationContext ctx) { + // Compatibility + if (ctx.meta.version < AstarSerializer.V4_3_68 && tiles != null) { + Dictionary conns = tiles.SelectMany(s => s.nodes).ToDictionary(n => n, n => n.connections ?? new Connection[0]); + // We need to recalculate all connections when upgrading data from earlier than 4.3.68 + // as the connections now need information about which edge was used and which edge it corresponds to in the neighbour. + // This may remove connections for e.g off-mesh links. + foreach (var tile in tiles) CreateNodeConnections(tile.nodes, false); + foreach (var tile in tiles) ConnectTileWithNeighbours(tile); + + // Restore any custom connections that were contained in the serialized file but didn't get added by the method calls above + GetNodes(node => { + var triNode = node as TriangleMeshNode; + foreach (var conn in conns[triNode].Where(conn => !triNode.ContainsOutgoingConnection(conn.node)).ToList()) { + triNode.AddPartialConnection(conn.node, conn.cost, conn.shapeEdgeInfo); + } + }); + } + + // Make sure that the transform is up to date. + // It is assumed that the current graph settings correspond to the correct + // transform as it is not serialized itself. + transform = CalculateTransform(); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavmeshBase.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavmeshBase.cs.meta new file mode 100644 index 0000000..eba3a2b --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/NavmeshBase.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: f8b76f19632144117a3ac7f28faf2c15 +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/Nodes.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes.meta new file mode 100644 index 0000000..78de162 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: e8a4fbed4121f4ae5a44468fdf1bce0e diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs new file mode 100644 index 0000000..fe40cb0 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs @@ -0,0 +1,447 @@ +#define PREALLOCATE_NODES +using System.Collections.Generic; +using Pathfinding.Serialization; +using Pathfinding.Util; +using UnityEngine; + +namespace Pathfinding { + /// Node used for the GridGraph + public class GridNode : GridNodeBase { + public GridNode() { + } + public GridNode (AstarPath astar) { + astar.InitializeNode(this); + } + +#if !ASTAR_NO_GRID_GRAPH + private static GridGraph[] _gridGraphs = new GridGraph[0]; + public static GridGraph GetGridGraph (uint graphIndex) { return _gridGraphs[(int)graphIndex]; } + + public static void SetGridGraph (int graphIndex, GridGraph graph) { + if (_gridGraphs.Length <= graphIndex) { + var gg = new GridGraph[graphIndex+1]; + for (int i = 0; i < _gridGraphs.Length; i++) gg[i] = _gridGraphs[i]; + _gridGraphs = gg; + } + + _gridGraphs[graphIndex] = graph; + } + + public static void ClearGridGraph (int graphIndex, GridGraph graph) { + if (graphIndex < _gridGraphs.Length && _gridGraphs[graphIndex] == graph) { + _gridGraphs[graphIndex] = null; + } + } + + /// Internal use only + internal ushort InternalGridFlags { + get { return gridFlags; } + set { gridFlags = value; } + } + + const int GridFlagsConnectionOffset = 0; + const int GridFlagsConnectionBit0 = 1 << GridFlagsConnectionOffset; + const int GridFlagsConnectionMask = 0xFF << GridFlagsConnectionOffset; + const int GridFlagsAxisAlignedConnectionMask = 0xF << GridFlagsConnectionOffset; + + const int GridFlagsEdgeNodeOffset = 10; + const int GridFlagsEdgeNodeMask = 1 << GridFlagsEdgeNodeOffset; + + public override bool HasConnectionsToAllEightNeighbours { + get { + return (InternalGridFlags & GridFlagsConnectionMask) == GridFlagsConnectionMask; + } + } + + public override bool HasConnectionsToAllAxisAlignedNeighbours { + get { + return (InternalGridFlags & GridFlagsAxisAlignedConnectionMask) == GridFlagsAxisAlignedConnectionMask; + } + } + + /// + /// True if the node has a connection in the specified direction. + /// The dir parameter corresponds to directions in the grid as: + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// See: + /// See: + /// See: + /// See: + /// See: + /// + public override bool HasConnectionInDirection (int dir) { + return (gridFlags >> dir & GridFlagsConnectionBit0) != 0; + } + + /// + /// True if the node has a connection in the specified direction. + /// Deprecated: Use + /// + [System.Obsolete("Use HasConnectionInDirection")] + public bool GetConnectionInternal (int dir) { + return HasConnectionInDirection(dir); + } + + /// + /// Enables or disables a connection in a specified direction on the graph. + /// + /// Note: This only changes the connection from this node to the other node. You may also want to call the same method on the other node with the opposite direction. + /// + /// See: + /// See: + /// + public void SetConnection (int dir, bool value) { + SetConnectionInternal(dir, value); + var grid = GetGridGraph(GraphIndex); + grid.nodeDataRef.connections[NodeInGridIndex] = (ulong)GetAllConnectionInternal(); + } + + /// + /// Enables or disables a connection in a specified direction on the graph. + /// See: + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead, for example . + /// + public void SetConnectionInternal (int dir, bool value) { + // Set bit number #dir to 1 or 0 depending on #value + unchecked { gridFlags = (ushort)(gridFlags & ~((ushort)1 << GridFlagsConnectionOffset << dir) | (value ? (ushort)1 : (ushort)0) << GridFlagsConnectionOffset << dir); } + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + /// + /// Sets the state of all grid connections. + /// + /// See: SetConnectionInternal + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead. + /// + /// a bitmask of the connections (bit 0 is the first connection, bit 1 the second connection, etc.). + public void SetAllConnectionInternal (int connections) { + unchecked { gridFlags = (ushort)((gridFlags & ~GridFlagsConnectionMask) | (connections << GridFlagsConnectionOffset)); } + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + /// Bitpacked int containing all 8 grid connections + public int GetAllConnectionInternal () { + return (gridFlags & GridFlagsConnectionMask) >> GridFlagsConnectionOffset; + } + + public override bool HasAnyGridConnections => GetAllConnectionInternal() != 0; + + /// + /// Disables all grid connections from this node. + /// Note: Other nodes might still be able to get to this node. + /// Therefore it is recommended to also disable the relevant connections on adjacent nodes. + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead. + /// + public override void ResetConnectionsInternal () { + unchecked { + gridFlags = (ushort)(gridFlags & ~GridFlagsConnectionMask); + } + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + /// + /// Work in progress for a feature that required info about which nodes were at the border of the graph. + /// Note: This property is not functional at the moment. + /// + public bool EdgeNode { + get { + return (gridFlags & GridFlagsEdgeNodeMask) != 0; + } + set { + unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsEdgeNodeMask | (value ? GridFlagsEdgeNodeMask : 0)); } + } + } + + public override GridNodeBase GetNeighbourAlongDirection (int direction) { + if (HasConnectionInDirection(direction)) { + GridGraph gg = GetGridGraph(GraphIndex); + return gg.nodes[NodeInGridIndex+gg.neighbourOffsets[direction]]; + } + return null; + } + + public override void ClearConnections (bool alsoReverse) { + if (alsoReverse) { + // Note: This assumes that all connections are bidirectional + // which should hold for all grid graphs unless some custom code has been added + for (int i = 0; i < 8; i++) { + var other = GetNeighbourAlongDirection(i) as GridNode; + if (other != null) { + // Remove reverse connection. See doc for GridGraph.neighbourOffsets to see which indices are used for what. + other.SetConnectionInternal(OppositeConnectionDirection(i), false); + } + } + } + + ResetConnectionsInternal(); + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + base.ClearConnections(alsoReverse); +#endif + } + + public override void GetConnections(GetConnectionsWithData action, ref T data, int connectionFilter) { + if ((connectionFilter & (Connection.IncomingConnection | Connection.OutgoingConnection)) == 0) return; + + GridGraph gg = GetGridGraph(GraphIndex); + + var neighbourOffsets = gg.neighbourOffsets; + var nodes = gg.nodes; + + for (int i = 0; i < 8; i++) { + if ((gridFlags >> i & GridFlagsConnectionBit0) != 0) { + var other = nodes[NodeInGridIndex + neighbourOffsets[i]]; + if (other != null) action(other, ref data); + } + } + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + base.GetConnections(action, ref data, connectionFilter); +#endif + } + + public override bool GetPortal (GraphNode other, out Vector3 left, out Vector3 right) { + if (other.GraphIndex != GraphIndex) { + left = right = Vector3.zero; + return false; + } + + GridGraph gg = GetGridGraph(GraphIndex); + var cellOffset = (other as GridNode).CoordinatesInGrid - CoordinatesInGrid; + var dir = OffsetToConnectionDirection(cellOffset.x, cellOffset.y); + if (dir == -1 || !HasConnectionInDirection(dir)) { + left = right = Vector3.zero; + return false; + } + + UnityEngine.Assertions.Assert.AreEqual(other, gg.nodes[NodeInGridIndex + gg.neighbourOffsets[dir]]); + + if (dir < 4) { + Vector3 middle = ((Vector3)(position + other.position))*0.5f; + Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position-position)); + cross.Normalize(); + cross *= gg.nodeSize*0.5f; + left = middle - cross; + right = middle + cross; + } else { + bool rClear = false; + bool lClear = false; + if (HasConnectionInDirection(dir-4)) { + var n2 = gg.nodes[NodeInGridIndex + gg.neighbourOffsets[dir-4]]; + if (n2.Walkable && n2.HasConnectionInDirection((dir-4+1)%4)) { + rClear = true; + } + } + + if (HasConnectionInDirection((dir-4+1)%4)) { + var n2 = gg.nodes[NodeInGridIndex + gg.neighbourOffsets[(dir-4+1)%4]]; + if (n2.Walkable && n2.HasConnectionInDirection(dir-4)) { + lClear = true; + } + } + + Vector3 middle = ((Vector3)(position + other.position))*0.5f; + Vector3 cross = Vector3.Cross(gg.collision.up, (Vector3)(other.position-position)); + cross.Normalize(); + cross *= gg.nodeSize*1.4142f; + left = middle - (lClear ? cross : Vector3.zero); + right = middle + (rClear ? cross : Vector3.zero); + } + return true; + } + + /// + /// Filters diagonal connections based on the non-diagonal ones to prevent corner cutting and similar things. + /// + /// This involves some complicated bitshifting to calculate which diagonal connections + /// should be active based on the non-diagonal ones. + /// For example a path should not be able to pass from A to B if the \ represent nodes + /// that we cannot traverse. + /// + /// + /// # B + /// A # + /// + /// + /// Additionally if corner cutting is disabled we will also prevent a connection from A to B in this case: + /// + /// + /// B + /// A # + /// + /// + /// If neighbours = 4 then only the 4 axis aligned connections will be enabled. + /// + /// If neighbours = 6 then only the connections which are valid for hexagonal graphs will be enabled. + /// + public static int FilterDiagonalConnections (int conns, NumNeighbours neighbours, bool cutCorners) { + switch (neighbours) { + case NumNeighbours.Four: + // The first 4 bits are the axis aligned connections + return conns & 0xF; + // Default case exists only to make the compiler happy, it is never intended to be used. + default: + case NumNeighbours.Eight: + if (cutCorners) { + int axisConns = conns & 0xF; + // If at least one axis aligned connection + // is adjacent to this diagonal, then we can add a connection. + // Bitshifting is a lot faster than calling node.HasConnectionInDirection. + // We need to check if connection i and i+1 are enabled + // but i+1 may overflow 4 and in that case need to be wrapped around + // (so 3+1 = 4 goes to 0). We do that by checking both connection i+1 + // and i+1-4 at the same time. Either i+1 or i+1-4 will be in the range + // from 0 to 4 (exclusive) + int diagConns = (axisConns | (axisConns >> 1 | axisConns << 3)) << 4; + + // Filter out diagonal connections that are invalid + // This will also filter out some junk bits which may be set to true above bit 8 + diagConns &= conns; + return axisConns | diagConns; + } else { + int axisConns = conns & 0xF; + // If exactly 2 axis aligned connections are adjacent to a diagonal connection + // then the diagonal connection is ok to use. + int diagConns = (axisConns & (axisConns >> 1 | axisConns << 3)) << 4; + + // Filter out diagonal connections that are invalid + // This will also filter out some junk bits which may be set above bit 8 + diagConns &= conns; + return axisConns | diagConns; + } + case NumNeighbours.Six: + // Hexagon layout + return conns & GridGraph.HexagonConnectionMask; + } + } + + public override void Open (Path path, uint pathNodeIndex, uint gScore) { + GridGraph gg = GetGridGraph(GraphIndex); + + int[] neighbourOffsets = gg.neighbourOffsets; + uint[] neighbourCosts = gg.neighbourCosts; + var nodes = gg.nodes; + var index = NodeInGridIndex; + + // Bitmask of the 8 connections out of this node + // Each bit represents one connection. + var conns = gridFlags & GridFlagsConnectionMask; + + // Loop over all connections, first the 4 axis aligned ones and then the 4 diagonal ones. + for (int dir = 0; dir < 8; dir++) { + // dir=4 is the first diagonal connection. + // At this point we know exactly which orthogonal (not diagonal) connections are actually traversable. + // So we do some filtering to determine which diagonals should be traversable. + // + // We do this dynamically because each path may use different tags or different + // ITraversalProviders that affect the result. + // + // When the grid graph is scanned this exact method is also run to pre-filter connections + // based on their walkability values. + // Doing pre-filtering is good because it allows users to use `HasConnectionInDirection` + // and it will return accurate values even for diagonals (even though it will of course not + // take into account any additional constraints such as tags or ITraversalProviders). + if (dir == 4 && (path.traversalProvider == null || path.traversalProvider.filterDiagonalGridConnections)) { + conns = FilterDiagonalConnections(conns, gg.neighbours, gg.cutCorners); + } + + // Check if we have a connection in this direction + if (((conns >> dir) & GridFlagsConnectionBit0) != 0) { + var other = nodes[index + neighbourOffsets[dir]]; + if (path.CanTraverse(this, other)) { + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, neighbourCosts[dir], 0, other.position); + } else { + // Mark that connection as not valid + conns &= ~(GridFlagsConnectionBit0 << dir); + } + } + } + + base.Open(path, pathNodeIndex, gScore); + } + + public override void SerializeNode (GraphSerializationContext ctx) { + base.SerializeNode(ctx); + ctx.SerializeInt3(position); + ctx.writer.Write(gridFlags); + } + + public override void DeserializeNode (GraphSerializationContext ctx) { + base.DeserializeNode(ctx); + position = ctx.DeserializeInt3(); + gridFlags = ctx.reader.ReadUInt16(); + } + + public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) { + // In case the node was already added as an internal grid connection, + // we need to remove that connection before we insert it as a custom connection. + // Using a custom connection is necessary because it has a custom cost. + if (node is GridNode gn && gn.GraphIndex == GraphIndex) { + RemoveGridConnection(gn); + } + base.AddPartialConnection(node, cost, isOutgoing, isIncoming); + } + + public override void RemovePartialConnection (GraphNode node) { + base.RemovePartialConnection(node); + // If the node is a grid node on the same graph, it might be added as an internal connection and not a custom one. + if (node is GridNode gn && gn.GraphIndex == GraphIndex) { + RemoveGridConnection(gn); + } + } + + /// + /// Removes a connection from the internal grid connections. + /// See: SetConnectionInternal + /// + protected void RemoveGridConnection (GridNode node) { + var nodeIndex = NodeInGridIndex; + var gg = GetGridGraph(GraphIndex); + + for (int i = 0; i < 8; i++) { + if (nodeIndex + gg.neighbourOffsets[i] == node.NodeInGridIndex && GetNeighbourAlongDirection(i) == node) { + SetConnectionInternal(i, false); + break; + } + } + } +#else + public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) { + throw new System.NotImplementedException(); + } + + public override void ClearConnections (bool alsoReverse) { + throw new System.NotImplementedException(); + } + + public override void GetConnections (GraphNodeDelegate del) { + throw new System.NotImplementedException(); + } + + public override void Open (Path path, PathNode pathNode, PathHandler handler) { + throw new System.NotImplementedException(); + } + + public override void AddPartialConnection (GraphNode node) { + throw new System.NotImplementedException(); + } +#endif + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs.meta new file mode 100644 index 0000000..bf0efe9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNode.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 5291f1ec332d746138ac025aecb1e12d +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs new file mode 100644 index 0000000..53673c1 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs @@ -0,0 +1,534 @@ +#define PREALLOCATE_NODES +using UnityEngine; +using Pathfinding.Serialization; + +namespace Pathfinding { + /// Base class for GridNode and LevelGridNode + public abstract class GridNodeBase : GraphNode { + const int GridFlagsWalkableErosionOffset = 8; + const int GridFlagsWalkableErosionMask = 1 << GridFlagsWalkableErosionOffset; + + const int GridFlagsWalkableTmpOffset = 9; + const int GridFlagsWalkableTmpMask = 1 << GridFlagsWalkableTmpOffset; + + public const int NodeInGridIndexLayerOffset = 24; + protected const int NodeInGridIndexMask = 0xFFFFFF; + + /// + /// Bitfield containing the x and z coordinates of the node as well as the layer (for layered grid graphs). + /// See: NodeInGridIndex + /// + protected int nodeInGridIndex; + protected ushort gridFlags; + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + /// + /// Custon non-grid connections from this node. + /// See: + /// See: + /// + /// This field is removed if the ASTAR_GRID_NO_CUSTOM_CONNECTIONS compiler directive is used. + /// Removing it can save a tiny bit of memory. You can enable the define in the Optimizations tab in the A* inspector. + /// See: compiler-directives (view in online documentation for working links) + /// + /// Note: If you modify this array or the contents of it you must call . + /// + public Connection[] connections; +#endif + + /// + /// The index of the node in the grid. + /// This is x + z*graph.width + /// So you can get the X and Z indices using + /// + /// int index = node.NodeInGridIndex; + /// int x = index % graph.width; + /// int z = index / graph.width; + /// // where graph is GridNode.GetGridGraph (node.graphIndex), i.e the graph the nodes are contained in. + /// + /// + /// See: + /// + public int NodeInGridIndex { get { return nodeInGridIndex & NodeInGridIndexMask; } set { nodeInGridIndex = (nodeInGridIndex & ~NodeInGridIndexMask) | value; } } + + /// + /// X coordinate of the node in the grid. + /// The node in the bottom left corner has (x,z) = (0,0) and the one in the opposite + /// corner has (x,z) = (width-1, depth-1) + /// + /// See: + /// See: + /// + public int XCoordinateInGrid => NodeInGridIndex % GridNode.GetGridGraph(GraphIndex).width; + + /// + /// Z coordinate of the node in the grid. + /// The node in the bottom left corner has (x,z) = (0,0) and the one in the opposite + /// corner has (x,z) = (width-1, depth-1) + /// + /// See: + /// See: + /// + public int ZCoordinateInGrid => NodeInGridIndex / GridNode.GetGridGraph(GraphIndex).width; + + /// + /// The X and Z coordinates of the node in the grid. + /// + /// The node in the bottom left corner has (x,z) = (0,0) and the one in the opposite + /// corner has (x,z) = (width-1, depth-1) + /// + /// See: + /// See: + /// See: + /// + public Int2 CoordinatesInGrid { + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + get { + var width = GridNode.GetGridGraph(GraphIndex).width; + var index = NodeInGridIndex; + var z = index / width; + var x = index - z * width; + return new Int2(x, z); + } + } + + /// + /// Stores walkability before erosion is applied. + /// Used internally when updating the graph. + /// + public bool WalkableErosion { + get { + return (gridFlags & GridFlagsWalkableErosionMask) != 0; + } + set { + unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsWalkableErosionMask | (value ? (ushort)GridFlagsWalkableErosionMask : (ushort)0)); } + } + } + + /// Temporary variable used internally when updating the graph. + public bool TmpWalkable { + get { + return (gridFlags & GridFlagsWalkableTmpMask) != 0; + } + set { + unchecked { gridFlags = (ushort)(gridFlags & ~GridFlagsWalkableTmpMask | (value ? (ushort)GridFlagsWalkableTmpMask : (ushort)0)); } + } + } + + /// + /// True if the node has grid connections to all its 8 neighbours. + /// Note: This will always return false if GridGraph.neighbours is set to anything other than Eight. + /// See: GetNeighbourAlongDirection + /// See: + /// + public abstract bool HasConnectionsToAllEightNeighbours { get; } + + /// + /// True if the node has grid connections to all its 4 axis-aligned neighbours. + /// See: GetNeighbourAlongDirection + /// See: + /// + public abstract bool HasConnectionsToAllAxisAlignedNeighbours { get; } + + /// + /// The connection opposite the given one. + /// + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// For example, dir=1 outputs 3, dir=6 outputs 4 and so on. + /// + /// See: + /// + public static int OppositeConnectionDirection (int dir) { + return dir < 4 ? ((dir + 2) % 4) : (((dir-2) % 4) + 4); + } + + /// + /// Converts from dx + 3*dz to a neighbour direction. + /// + /// Used by . + /// + /// Assumes that dx and dz are both in the range [0,2]. + /// See: + /// + internal static readonly int[] offsetToDirection = { 7, 0, 4, 3, -1, 1, 6, 2, 5 }; + + /// + /// Converts from a delta (dx, dz) to a neighbour direction. + /// + /// For example, if dx=1 and dz=0, the return value will be 1, which is the direction to the right of a grid coordinate. + /// + /// If dx=0 and dz=0, the return value will be -1. + /// + /// See: + /// + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// See: + /// + /// X coordinate delta. Should be in the range [-1, 1]. Values outside this range will cause -1 to be returned. + /// Z coordinate delta. Should be in the range [-1, 1]. Values outside this range will cause -1 to be returned. + public static int OffsetToConnectionDirection (int dx, int dz) { + dx++; dz++; + if ((uint)dx > 2 || (uint)dz > 2) return -1; + return offsetToDirection[3*dz + dx]; + } + + /// + /// Projects the given point onto the plane of this node's surface. + /// + /// The point will be projected down to a plane that contains the surface of the node. + /// If the point is not contained inside the node, it is projected down onto this plane anyway. + /// + public Vector3 ProjectOnSurface (Vector3 point) { + var gg = GridNode.GetGridGraph(GraphIndex); + var nodeCenter = (Vector3)position; + var up = gg.transform.WorldUpAtGraphPosition(nodeCenter); + return point - up * Vector3.Dot(up, point - nodeCenter); + } + + public override Vector3 ClosestPointOnNode (Vector3 p) { + var gg = GridNode.GetGridGraph(GraphIndex); + + // Convert to graph space + var nodeCenter = (Vector3)position; + // Calculate the offset from the node's center to the given point in graph space + var offsetInGraphSpace = gg.transform.InverseTransformVector(p - nodeCenter); + // Project onto the node's surface + offsetInGraphSpace.y = 0; + // Clamp to the node's extents + offsetInGraphSpace.x = Mathf.Clamp(offsetInGraphSpace.x, -0.5f, 0.5f); + offsetInGraphSpace.z = Mathf.Clamp(offsetInGraphSpace.z, -0.5f, 0.5f); + // Convert back to world space + return nodeCenter + gg.transform.TransformVector(offsetInGraphSpace); + } + + /// + /// Checks if point is inside the node when seen from above. + /// + /// The borders of a node are considered to be inside the node. + /// + /// Note that is faster than this method as it avoids + /// some coordinate transformations. If you are repeatedly calling this method + /// on many different nodes but with the same point then you should consider + /// transforming the point first and then calling ContainsPointInGraphSpace. + /// + /// Int3 p = (Int3)graph.transform.InverseTransform(point); + /// + /// node.ContainsPointInGraphSpace(p); + /// + /// + public override bool ContainsPoint (Vector3 point) { + var gg = Graph as GridGraph; + // Convert to graph space + return ContainsPointInGraphSpace((Int3)gg.transform.InverseTransform(point)); + } + + /// + /// Checks if point is inside the node in graph space. + /// + /// The borders of a node are considered to be inside the node. + /// + /// The y coordinate of the point is ignored. + /// + public override bool ContainsPointInGraphSpace (Int3 point) { + // Calculate graph position of this node + var x = XCoordinateInGrid*Int3.Precision; + var z = ZCoordinateInGrid*Int3.Precision; + + return point.x >= x && point.x <= x + Int3.Precision && point.z >= z && point.z <= z + Int3.Precision; + } + + public override float SurfaceArea () { + GridGraph gg = GridNode.GetGridGraph(GraphIndex); + + return gg.nodeSize*gg.nodeSize; + } + + public override Vector3 RandomPointOnSurface () { + GridGraph gg = GridNode.GetGridGraph(GraphIndex); + + var graphSpacePosition = gg.transform.InverseTransform((Vector3)position); + + var r = AstarMath.ThreadSafeRandomFloat2(); + return gg.transform.Transform(graphSpacePosition + new Vector3(r.x - 0.5f, 0, r.y - 0.5f)); + } + + /// + /// Transforms a world space point to a normalized point on this node's surface. + /// (0.5,0.5) represents the node's center. (0,0), (1,0), (1,1) and (0,1) each represent the corners of the node. + /// + /// See: + /// + public Vector2 NormalizePoint (Vector3 worldPoint) { + GridGraph gg = GridNode.GetGridGraph(GraphIndex); + var graphSpacePosition = gg.transform.InverseTransform(worldPoint); + + return new Vector2(graphSpacePosition.x - this.XCoordinateInGrid, graphSpacePosition.z - this.ZCoordinateInGrid); + } + + /// + /// Transforms a normalized point on this node's surface to a world space point. + /// (0.5,0.5) represents the node's center. (0,0), (1,0), (1,1) and (0,1) each represent the corners of the node. + /// + /// See: + /// + public Vector3 UnNormalizePoint (Vector2 normalizedPointOnSurface) { + GridGraph gg = GridNode.GetGridGraph(GraphIndex); + + return (Vector3)this.position + gg.transform.TransformVector(new Vector3(normalizedPointOnSurface.x - 0.5f, 0, normalizedPointOnSurface.y - 0.5f)); + } + + public override int GetGizmoHashCode () { + var hash = base.GetGizmoHashCode(); + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + hash ^= 17 * connections[i].GetHashCode(); + } + } +#endif + hash ^= 109 * gridFlags; + return hash; + } + + /// + /// Adjacent grid node in the specified direction. + /// This will return null if the node does not have a connection to a node + /// in that direction. + /// + /// The dir parameter corresponds to directions in the grid as: + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// See: + /// See: + /// + /// Note: This method only takes grid connections into account, not custom connections (i.e. those added using or using node links). + /// + public abstract GridNodeBase GetNeighbourAlongDirection(int direction); + + /// + /// True if the node has a connection to an adjecent node in the specified direction. + /// + /// The dir parameter corresponds to directions in the grid as: + /// + /// Z + /// | + /// | + /// + /// 6 2 5 + /// \ | / + /// -- 3 - X - 1 ----- X + /// / | \ + /// 7 0 4 + /// + /// | + /// | + /// + /// + /// See: + /// See: + /// See: + /// + /// Note: This method only takes grid connections into account, not custom connections (i.e. those added using or using node links). + /// + public virtual bool HasConnectionInDirection (int direction) { + // TODO: Can be optimized if overriden in each subclass + return GetNeighbourAlongDirection(direction) != null; + } + + /// True if this node has any grid connections + public abstract bool HasAnyGridConnections { get; } + + public override bool ContainsOutgoingConnection (GraphNode node) { +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + if (connections[i].node == node && connections[i].isOutgoing) { + return true; + } + } + } +#endif + + for (int i = 0; i < 8; i++) { + if (node == GetNeighbourAlongDirection(i)) { + return true; + } + } + + return false; + } + + /// + /// Disables all grid connections from this node. + /// Note: Other nodes might still be able to get to this node. + /// Therefore it is recommended to also disable the relevant connections on adjacent nodes. + /// + public abstract void ResetConnectionsInternal(); + + public override void OpenAtPoint (Path path, uint pathNodeIndex, Int3 pos, uint gScore) { + path.OpenCandidateConnectionsToEndNode(pos, pathNodeIndex, pathNodeIndex, gScore); + path.OpenCandidateConnection(pathNodeIndex, NodeIndex, gScore, 0, 0, position); + } + + public override void Open (Path path, uint pathNodeIndex, uint gScore) { + path.OpenCandidateConnectionsToEndNode(position, pathNodeIndex, pathNodeIndex, gScore); + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + GraphNode other = connections[i].node; + if (!connections[i].isOutgoing || !path.CanTraverse(this, other)) continue; + + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, connections[i].cost, 0, other.position); + } + } +#endif + } + +#if ASTAR_GRID_NO_CUSTOM_CONNECTIONS + public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) { + throw new System.NotImplementedException("GridNodes do not have support for adding manual connections with your current settings."+ + "\nPlease disable ASTAR_GRID_NO_CUSTOM_CONNECTIONS in the Optimizations tab in the A* Inspector"); + } + + public override void RemovePartialConnection (GraphNode node) { + // Nothing to do because ASTAR_GRID_NO_CUSTOM_CONNECTIONS is enabled + } + + public void ClearCustomConnections (bool alsoReverse) { + } +#else + /// Same as , but does not clear grid connections, only custom ones (e.g added by or a NodeLink component) + public void ClearCustomConnections (bool alsoReverse) { + if (connections != null) { + for (int i = 0; i < connections.Length; i++) connections[i].node.RemovePartialConnection(this); + connections = null; + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + } + + public override void ClearConnections (bool alsoReverse) { + ClearCustomConnections(alsoReverse); + } + + public override void GetConnections(GetConnectionsWithData action, ref T data, int connectionFilter) { + if (connections == null) return; + for (int i = 0; i < connections.Length; i++) if ((connections[i].shapeEdgeInfo & connectionFilter) != 0) action(connections[i].node, ref data); + } + + public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) { + if (node == null) throw new System.ArgumentNullException(); + + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + if (connections[i].node == node) { + connections[i].cost = cost; + connections[i].shapeEdgeInfo = Connection.PackShapeEdgeInfo(isOutgoing, isIncoming); + return; + } + } + } + + int connLength = connections != null ? connections.Length : 0; + + var newconns = new Connection[connLength+1]; + for (int i = 0; i < connLength; i++) { + newconns[i] = connections[i]; + } + + newconns[connLength] = new Connection(node, cost, isOutgoing, isIncoming); + + connections = newconns; + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + /// + /// Removes any connection from this node to the specified node. + /// If no such connection exists, nothing will be done. + /// + /// Note: This only removes the connection from this node to the other node. + /// You may want to call the same function on the other node to remove its eventual connection + /// to this node. + /// + /// Version: Before 4.3.48 This method only handled custom connections (those added using link components or the AddConnection method). + /// Regular grid connections had to be added or removed using . Starting with 4.3.48 this method + /// can remove all types of connections. + /// + public override void RemovePartialConnection (GraphNode node) { + if (connections == null) return; + + for (int i = 0; i < connections.Length; i++) { + if (connections[i].node == node) { + int connLength = connections.Length; + + var newconns = new Connection[connLength-1]; + for (int j = 0; j < i; j++) { + newconns[j] = connections[j]; + } + for (int j = i+1; j < connLength; j++) { + newconns[j-1] = connections[j]; + } + + connections = newconns; + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + return; + } + } + } + + public override void SerializeReferences (GraphSerializationContext ctx) { + ctx.SerializeConnections(connections, true); + } + + public override void DeserializeReferences (GraphSerializationContext ctx) { + // Grid nodes didn't serialize references before 3.8.3 + if (ctx.meta.version < AstarSerializer.V3_8_3) + return; + + connections = ctx.DeserializeConnections(ctx.meta.version >= AstarSerializer.V4_3_85); + } +#endif + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs.meta new file mode 100644 index 0000000..819cad4 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/GridNodeBase.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 1b723ddfaa37b46eca23cd8d042ad3e9 +timeCreated: 1459629300 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs new file mode 100644 index 0000000..3a41a08 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs @@ -0,0 +1,403 @@ +#if !ASTAR_NO_GRID_GRAPH +#if !ASTAR_LEVELGRIDNODE_MORE_LAYERS +#define ASTAR_LEVELGRIDNODE_FEW_LAYERS +#endif +using UnityEngine; +using System.Collections.Generic; +using Pathfinding.Serialization; + +namespace Pathfinding { + /// + /// Describes a single node for the LayerGridGraph. + /// Works almost the same as a grid node, except that it also stores to which layer the connections go to + /// + public class LevelGridNode : GridNodeBase { + public LevelGridNode() { + } + + public LevelGridNode (AstarPath astar) { + astar.InitializeNode(this); + } + + private static LayerGridGraph[] _gridGraphs = new LayerGridGraph[0]; + public static LayerGridGraph GetGridGraph (uint graphIndex) { return _gridGraphs[(int)graphIndex]; } + + public static void SetGridGraph (int graphIndex, LayerGridGraph graph) { + // LayeredGridGraphs also show up in the grid graph list + // This is required by e.g the XCoordinateInGrid properties + GridNode.SetGridGraph(graphIndex, graph); + if (_gridGraphs.Length <= graphIndex) { + var newGraphs = new LayerGridGraph[graphIndex+1]; + for (int i = 0; i < _gridGraphs.Length; i++) newGraphs[i] = _gridGraphs[i]; + _gridGraphs = newGraphs; + } + + _gridGraphs[graphIndex] = graph; + } + + public static void ClearGridGraph (int graphIndex, LayerGridGraph graph) { + if (graphIndex < _gridGraphs.Length && _gridGraphs[graphIndex] == graph) { + _gridGraphs[graphIndex] = null; + } + } + +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + public uint gridConnections; +#else + public ulong gridConnections; +#endif + + protected static LayerGridGraph[] gridGraphs; + + const int MaxNeighbours = 8; +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + public const int ConnectionMask = 0xF; + public const int ConnectionStride = 4; + public const int AxisAlignedConnectionsMask = 0xFFFF; + public const uint AllConnectionsMask = 0xFFFFFFFF; +#else + public const int ConnectionMask = 0xFF; + public const int ConnectionStride = 8; + public const ulong AxisAlignedConnectionsMask = 0xFFFFFFFF; + public const ulong AllConnectionsMask = 0xFFFFFFFFFFFFFFFF; +#endif + public const int NoConnection = ConnectionMask; + + internal const ulong DiagonalConnectionsMask = ((ulong)NoConnection << 4*ConnectionStride) | ((ulong)NoConnection << 5*ConnectionStride) | ((ulong)NoConnection << 6*ConnectionStride) | ((ulong)NoConnection << 7*ConnectionStride); + + /// + /// Maximum number of layers the layered grid graph supports. + /// + /// This can be changed in the A* Inspector -> Optimizations tab by enabling or disabling the ASTAR_LEVELGRIDNODE_MORE_LAYERS option. + /// + public const int MaxLayerCount = ConnectionMask; + + /// + /// Removes all grid connections from this node. + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead. + /// + public override void ResetConnectionsInternal () { +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + gridConnections = unchecked ((uint)-1); +#else + gridConnections = unchecked ((ulong)-1); +#endif + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + public override bool HasAnyGridConnections => gridConnections != unchecked ((uint)-1); +#else + public override bool HasAnyGridConnections => gridConnections != unchecked ((ulong)-1); +#endif + + public override bool HasConnectionsToAllEightNeighbours { + get { + for (int i = 0; i < 8; i++) { + if (!HasConnectionInDirection(i)) return false; + } + return true; + } + } + + public override bool HasConnectionsToAllAxisAlignedNeighbours { + get { + return (gridConnections & AxisAlignedConnectionsMask) == AxisAlignedConnectionsMask; + } + } + + /// + /// Layer coordinate of the node in the grid. + /// If there are multiple nodes in the same (x,z) cell, then they will be stored in different layers. + /// Together with NodeInGridIndex, you can look up the node in the nodes array + /// + /// int index = node.NodeInGridIndex + node.LayerCoordinateInGrid * graph.width * graph.depth; + /// Assert(node == graph.nodes[index]); + /// + /// + /// See: XCoordInGrid + /// See: ZCoordInGrid + /// See: NodeInGridIndex + /// + public int LayerCoordinateInGrid { get { return nodeInGridIndex >> NodeInGridIndexLayerOffset; } set { nodeInGridIndex = (nodeInGridIndex & NodeInGridIndexMask) | (value << NodeInGridIndexLayerOffset); } } + + public void SetPosition (Int3 position) { + this.position = position; + } + + public override int GetGizmoHashCode () { + return base.GetGizmoHashCode() ^ (int)((805306457UL * gridConnections) ^ (402653189UL * (gridConnections >> 32))); + } + + public override GridNodeBase GetNeighbourAlongDirection (int direction) { + int conn = GetConnectionValue(direction); + + if (conn != NoConnection) { + LayerGridGraph graph = GetGridGraph(GraphIndex); + return graph.nodes[NodeInGridIndex+graph.neighbourOffsets[direction] + graph.lastScannedWidth*graph.lastScannedDepth*conn]; + } + return null; + } + + public override void ClearConnections (bool alsoReverse) { + if (alsoReverse) { + LayerGridGraph graph = GetGridGraph(GraphIndex); + int[] neighbourOffsets = graph.neighbourOffsets; + var nodes = graph.nodes; + + for (int i = 0; i < MaxNeighbours; i++) { + int conn = GetConnectionValue(i); + if (conn != LevelGridNode.NoConnection) { + var other = nodes[NodeInGridIndex+neighbourOffsets[i] + graph.lastScannedWidth*graph.lastScannedDepth*conn] as LevelGridNode; + if (other != null) { + // Remove reverse connection + other.SetConnectionValue((i + 2) % 4, NoConnection); + } + } + } + } + + ResetConnectionsInternal(); + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + base.ClearConnections(alsoReverse); +#endif + } + + public override void GetConnections(GetConnectionsWithData action, ref T data, int connectionFilter) { + if ((connectionFilter & (Connection.IncomingConnection | Connection.OutgoingConnection)) == 0) return; + + LayerGridGraph graph = GetGridGraph(GraphIndex); + + int[] neighbourOffsets = graph.neighbourOffsets; + var nodes = graph.nodes; + int index = NodeInGridIndex; + + for (int i = 0; i < MaxNeighbours; i++) { + int conn = GetConnectionValue(i); + if (conn != LevelGridNode.NoConnection) { + var other = nodes[index+neighbourOffsets[i] + graph.lastScannedWidth*graph.lastScannedDepth*conn]; + if (other != null) action(other, ref data); + } + } + +#if !ASTAR_GRID_NO_CUSTOM_CONNECTIONS + base.GetConnections(action, ref data, connectionFilter); +#endif + } + + /// + /// Is there a grid connection in that direction. + /// + /// Deprecated: Use instead + /// + [System.Obsolete("Use HasConnectionInDirection instead")] + public bool GetConnection (int i) { + return ((gridConnections >> i*ConnectionStride) & ConnectionMask) != NoConnection; + } + + public override bool HasConnectionInDirection (int direction) { + return ((gridConnections >> direction*ConnectionStride) & ConnectionMask) != NoConnection; + } + + /// + /// Set which layer a grid connection goes to. + /// + /// Warning: Using this method can make the graph data inconsistent. It's recommended to use other ways to update the graph, instead. + /// + /// Direction for the connection. + /// The layer of the connected node or #NoConnection if there should be no connection in that direction. + public void SetConnectionValue (int dir, int value) { +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + gridConnections = gridConnections & ~(((uint)NoConnection << dir*ConnectionStride)) | ((uint)value << dir*ConnectionStride); +#else + gridConnections = gridConnections & ~(((ulong)NoConnection << dir*ConnectionStride)) | ((ulong)value << dir*ConnectionStride); +#endif + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + public void SetAllConnectionInternal (ulong value) { + gridConnections = (uint)value; + } +#else + public void SetAllConnectionInternal (ulong value) { + gridConnections = value; + } +#endif + + + /// + /// Which layer a grid connection goes to. + /// Returns: The layer of the connected node or if there is no connection in that direction. + /// + /// Direction for the connection. + public int GetConnectionValue (int dir) { + return (int)((gridConnections >> dir*ConnectionStride) & ConnectionMask); + } + + public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) { + // In case the node was already added as an internal grid connection, + // we need to remove that connection before we insert it as a custom connection. + // Using a custom connection is necessary because it has a custom cost. + if (node is LevelGridNode gn && gn.GraphIndex == GraphIndex) { + RemoveGridConnection(gn); + } + base.AddPartialConnection(node, cost, isOutgoing, isIncoming); + } + + public override void RemovePartialConnection (GraphNode node) { + base.RemovePartialConnection(node); + // If the node is a grid node on the same graph, it might be added as an internal connection and not a custom one. + if (node is LevelGridNode gn && gn.GraphIndex == GraphIndex) { + RemoveGridConnection(gn); + } + } + + /// + /// Removes a connection from the internal grid connections, not the list of custom connections. + /// See: SetConnectionValue + /// + protected void RemoveGridConnection (LevelGridNode node) { + var nodeIndex = NodeInGridIndex; + var gg = GetGridGraph(GraphIndex); + + for (int i = 0; i < 8; i++) { + if (nodeIndex + gg.neighbourOffsets[i] == node.NodeInGridIndex && GetNeighbourAlongDirection(i) == node) { + SetConnectionValue(i, NoConnection); + break; + } + } + } + + public override bool GetPortal (GraphNode other, out Vector3 left, out Vector3 right) { + LayerGridGraph graph = GetGridGraph(GraphIndex); + int[] neighbourOffsets = graph.neighbourOffsets; + var nodes = graph.nodes; + int index = NodeInGridIndex; + + for (int i = 0; i < MaxNeighbours; i++) { + int conn = GetConnectionValue(i); + if (conn != LevelGridNode.NoConnection) { + if (other == nodes[index+neighbourOffsets[i] + graph.lastScannedWidth*graph.lastScannedDepth*conn]) { + Vector3 middle = ((Vector3)(position + other.position))*0.5f; + Vector3 cross = Vector3.Cross(graph.collision.up, (Vector3)(other.position-position)); + cross.Normalize(); + cross *= graph.nodeSize*0.5f; + left = middle - cross; + right = middle + cross; + return true; + } + } + } + + left = Vector3.zero; + right = Vector3.zero; + return false; + } + + public override void Open (Path path, uint pathNodeIndex, uint gScore) { + LayerGridGraph graph = GetGridGraph(GraphIndex); + + int[] neighbourOffsets = graph.neighbourOffsets; + uint[] neighbourCosts = graph.neighbourCosts; + var nodes = graph.nodes; + int index = NodeInGridIndex; + + // Bitmask of the 8 connections out of this node. + // Each bit represents one connection. + // We only use this to be able to dynamically handle + // things like cutCorners and other diagonal connection filtering + // based on things like the tags or ITraversalProvider set for just this path. + // It starts off with all connections enabled but then in the following loop + // we will remove connections which are not traversable. + // When we get to the first diagonal connection we run a pass to + // filter out any diagonal connections which shouldn't be enabled. + // See the documentation for FilterDiagonalConnections for more info. + // The regular grid graph does a similar thing. + var conns = 0xFF; + + for (int dir = 0; dir < MaxNeighbours; dir++) { + if (dir == 4 && (path.traversalProvider == null || path.traversalProvider.filterDiagonalGridConnections)) { + conns = GridNode.FilterDiagonalConnections(conns, graph.neighbours, graph.cutCorners); + } + + int conn = GetConnectionValue(dir); + if (conn != LevelGridNode.NoConnection && ((conns >> dir) & 0x1) != 0) { + GraphNode other = nodes[index+neighbourOffsets[dir] + graph.lastScannedWidth*graph.lastScannedDepth*conn]; + + if (!path.CanTraverse(this, other)) { + conns &= ~(1 << dir); + continue; + } + + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, neighbourCosts[dir], 0, other.position); + } else { + conns &= ~(1 << dir); + } + } + + base.Open(path, pathNodeIndex, gScore); + } + + public override void SerializeNode (GraphSerializationContext ctx) { + base.SerializeNode(ctx); + ctx.SerializeInt3(position); + ctx.writer.Write(gridFlags); + // gridConnections are now always serialized as 64 bits for easier compatibility handling +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + // Convert from 32 bits to 64-bits + ulong connectionsLong = 0; + for (int i = 0; i < 8; i++) connectionsLong |= (ulong)GetConnectionValue(i) << (i*8); +#else + ulong connectionsLong = gridConnections; +#endif + ctx.writer.Write(connectionsLong); + } + + public override void DeserializeNode (GraphSerializationContext ctx) { + base.DeserializeNode(ctx); + position = ctx.DeserializeInt3(); + gridFlags = ctx.reader.ReadUInt16(); + if (ctx.meta.version < AstarSerializer.V4_3_12) { + // Note: assumes ASTAR_LEVELGRIDNODE_FEW_LAYERS was false when saving, which was the default + // This info not saved with the graph unfortunately and in 4.3.12 the default changed. + ulong conns; + if (ctx.meta.version < AstarSerializer.V3_9_0) { + // Set the upper 32 bits for compatibility + conns = ctx.reader.ReadUInt32() | (((ulong)NoConnection << 56) | ((ulong)NoConnection << 48) | ((ulong)NoConnection << 40) | ((ulong)NoConnection << 32)); + } else { + conns = ctx.reader.ReadUInt64(); + } + const int stride = 8; + const int mask = (1 << stride) - 1; + gridConnections = 0; + for (int i = 0; i < 8; i++) { + var y = (conns >> (i*stride)) & mask; + // 4.3.12 by default only supports 15 layers. So we may have to disable some connections when loading from earlier versions. + if ((y & ConnectionMask) != y) y = NoConnection; + SetConnectionValue(i, (int)y); + } + } else { + var gridConnectionsLong = ctx.reader.ReadUInt64(); +#if ASTAR_LEVELGRIDNODE_FEW_LAYERS + uint c = 0; + if (ctx.meta.version < AstarSerializer.V4_3_83) { + // The default during 4.3.12..4.3.83 was that ASTAR_LEVELGRIDNODE_FEW_LAYERS was enabled, but it was serialized just as 32-bits zero-extended to 64 bits + c = (uint)gridConnectionsLong; + } else { + // Convert from 64 bits to 32-bits + for (int i = 0; i < 8; i++) { + c |= ((uint)(gridConnectionsLong >> (i*8)) & LevelGridNode.ConnectionMask) << (LevelGridNode.ConnectionStride*i); + } + } + gridConnections = c; +#else + gridConnections = gridConnectionsLong; +#endif + } + } + } +} +#endif diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs.meta new file mode 100644 index 0000000..feb761d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/LevelGridNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e6b8ae51fad7eec4198819399a4e33a2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs new file mode 100644 index 0000000..852fae8 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs @@ -0,0 +1,220 @@ +using UnityEngine; +using Pathfinding.Serialization; + +namespace Pathfinding { + /// + /// Node used for the PointGraph. + /// This is just a simple point with a list of connections (and associated costs) to other nodes. + /// It does not have any concept of a surface like many other node types. + /// + /// See: PointGraph + /// + public class PointNode : GraphNode { + /// + /// All connections from this node. + /// See: + /// See: + /// + /// Note: If you modify this array or the contents of it you must call . + /// + /// Note: If you modify this array or the contents of it you must call with the length of the new connections. + /// + public Connection[] connections; + + /// + /// GameObject this node was created from (if any). + /// Warning: When loading a graph from a saved file or from cache, this field will be null. + /// + /// + /// var node = AstarPath.active.GetNearest(transform.position).node; + /// var pointNode = node as PointNode; + /// + /// if (pointNode != null) { + /// Debug.Log("That node was created from the GameObject named " + pointNode.gameObject.name); + /// } else { + /// Debug.Log("That node is not a PointNode"); + /// } + /// + /// + public GameObject gameObject; + + public void SetPosition (Int3 value) { + position = value; + } + + public PointNode() { } + public PointNode (AstarPath astar) { + astar.InitializeNode(this); + } + + /// + /// Closest point on the surface of this node to the point p. + /// + /// For a point node this is always the node's sicne it has no surface. + /// + public override Vector3 ClosestPointOnNode (Vector3 p) { + return (Vector3)this.position; + } + + /// + /// Checks if point is inside the node when seen from above. + /// + /// Since point nodes have no surface area, this method always returns false. + /// + public override bool ContainsPoint (Vector3 point) { + return false; + } + + /// + /// Checks if point is inside the node in graph space. + /// + /// Since point nodes have no surface area, this method always returns false. + /// + public override bool ContainsPointInGraphSpace (Int3 point) { + return false; + } + + public override void GetConnections(GetConnectionsWithData action, ref T data, int connectionFilter) { + if (connections == null) return; + for (int i = 0; i < connections.Length; i++) if ((connections[i].shapeEdgeInfo & connectionFilter) != 0) action(connections[i].node, ref data); + } + + public override void ClearConnections (bool alsoReverse) { + if (alsoReverse && connections != null) { + for (int i = 0; i < connections.Length; i++) { + connections[i].node.RemovePartialConnection(this); + } + } + + connections = null; + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + } + + public override bool ContainsOutgoingConnection (GraphNode node) { + if (connections == null) return false; + for (int i = 0; i < connections.Length; i++) if (connections[i].node == node && connections[i].isOutgoing) return true; + return false; + } + + public override void AddPartialConnection (GraphNode node, uint cost, bool isOutgoing, bool isIncoming) { + if (node == null) throw new System.ArgumentNullException(); + + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + if (connections[i].node == node) { + connections[i].cost = cost; + connections[i].shapeEdgeInfo = Connection.PackShapeEdgeInfo(isOutgoing, isIncoming); + return; + } + } + } + + int connLength = connections != null ? connections.Length : 0; + + var newconns = new Connection[connLength+1]; + for (int i = 0; i < connLength; i++) { + newconns[i] = connections[i]; + } + + newconns[connLength] = new Connection(node, cost, isOutgoing, isIncoming); + + connections = newconns; + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + + // Make sure the graph knows that there exists a connection with this length + if (this.Graph is PointGraph pg) pg.RegisterConnectionLength((node.position - position).sqrMagnitudeLong); + } + + public override void RemovePartialConnection (GraphNode node) { + if (connections == null) return; + + for (int i = 0; i < connections.Length; i++) { + if (connections[i].node == node) { + int connLength = connections.Length; + + var newconns = new Connection[connLength-1]; + for (int j = 0; j < i; j++) { + newconns[j] = connections[j]; + } + for (int j = i+1; j < connLength; j++) { + newconns[j-1] = connections[j]; + } + + connections = newconns; + AstarPath.active.hierarchicalGraph.AddDirtyNode(this); + return; + } + } + } + + public override void Open (Path path, uint pathNodeIndex, uint gScore) { + path.OpenCandidateConnectionsToEndNode(position, pathNodeIndex, pathNodeIndex, gScore); + + if (connections == null) return; + + for (int i = 0; i < connections.Length; i++) { + GraphNode other = connections[i].node; + + if (connections[i].isOutgoing && path.CanTraverse(this, other)) { + if (other is PointNode) { + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, connections[i].cost, 0, other.position); + } else { + // When connecting to a non-point node, use a special function to open the connection. + // The typical case for this is that we are at the end of an off-mesh link and we are connecting to a navmesh node. + // In that case, this node's position is in the interior of the navmesh node. We let the navmesh node decide how + // that should be handled. + other.OpenAtPoint(path, pathNodeIndex, position, gScore); + } + } + } + } + + public override void OpenAtPoint (Path path, uint pathNodeIndex, Int3 pos, uint gScore) { + if (path.CanTraverse(this)) { + // TODO: Ideally we should only allow connections to the temporary end node directly from the temporary start node + // iff they lie on the same connection edge. Otherwise we need to pass through the center of this node. + // + // N1---E----N2 + // | / + // | / + // S + // | + // N3 + // + path.OpenCandidateConnectionsToEndNode(pos, pathNodeIndex, pathNodeIndex, gScore); + + var cost = (uint)(pos - this.position).costMagnitude; + path.OpenCandidateConnection(pathNodeIndex, NodeIndex, gScore, cost, 0, position); + } + } + + public override int GetGizmoHashCode () { + var hash = base.GetGizmoHashCode(); + + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + hash ^= 17 * connections[i].GetHashCode(); + } + } + return hash; + } + + public override void SerializeNode (GraphSerializationContext ctx) { + base.SerializeNode(ctx); + ctx.SerializeInt3(position); + } + + public override void DeserializeNode (GraphSerializationContext ctx) { + base.DeserializeNode(ctx); + position = ctx.DeserializeInt3(); + } + + public override void SerializeReferences (GraphSerializationContext ctx) { + ctx.SerializeConnections(connections, true); + } + + public override void DeserializeReferences (GraphSerializationContext ctx) { + connections = ctx.DeserializeConnections(ctx.meta.version >= AstarSerializer.V4_3_85); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs.meta new file mode 100644 index 0000000..00325c7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/PointNode.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 986ad6174b59e40068c715a916740ce9 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs new file mode 100644 index 0000000..e7142a6 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs @@ -0,0 +1,673 @@ +#pragma warning disable 0162 +using UnityEngine; +using Pathfinding.Serialization; +using UnityEngine.Assertions; +using Unity.Mathematics; +using Pathfinding.Util; +using Unity.Burst; + +namespace Pathfinding { + /// Interface for something that holds a triangle based navmesh + public interface INavmeshHolder : ITransformedGraph, INavmesh { + /// Position of vertex number i in the world + Int3 GetVertex(int i); + + /// + /// Position of vertex number i in coordinates local to the graph. + /// The up direction is always the +Y axis for these coordinates. + /// + Int3 GetVertexInGraphSpace(int i); + + int GetVertexArrayIndex(int index); + + /// Transforms coordinates from graph space to world space + void GetTileCoordinates(int tileIndex, out int x, out int z); + } + + /// Node represented by a triangle + [Unity.Burst.BurstCompile] + // Sealing the class provides a nice performance boost (~5-10%) during pathfinding, because the JIT can inline more things and use non-virtual calls. + public sealed class TriangleMeshNode : MeshNode { + public TriangleMeshNode () { + HierarchicalNodeIndex = 0; + NodeIndex = DestroyedNodeIndex; + } + + public TriangleMeshNode (AstarPath astar) { + astar.InitializeNode(this); + } + + /// + /// Legacy compatibility. + /// Enabling this will make pathfinding use node centers, which leads to less accurate paths (but it's faster). + /// + public const bool InaccuratePathSearch = false; + internal override int PathNodeVariants => InaccuratePathSearch ? 1 : 3; + + /// Internal vertex index for the first vertex + public int v0; + + /// Internal vertex index for the second vertex + public int v1; + + /// Internal vertex index for the third vertex + public int v2; + + /// Holds INavmeshHolder references for all graph indices to be able to access them in a performant manner + static INavmeshHolder[] _navmeshHolders = new INavmeshHolder[0]; + + /// Used for synchronised access to the array + static readonly System.Object lockObject = new System.Object(); + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public static INavmeshHolder GetNavmeshHolder (uint graphIndex) { + return _navmeshHolders[(int)graphIndex]; + } + + /// + /// Tile index in the recast or navmesh graph that this node is part of. + /// See: + /// + public int TileIndex => (v0 >> NavmeshBase.TileIndexOffset) & NavmeshBase.TileIndexMask; + + /// + /// Sets the internal navmesh holder for a given graph index. + /// Warning: Internal method + /// + public static void SetNavmeshHolder (int graphIndex, INavmeshHolder graph) { + // We need to lock to make sure that + // the resize operation is thread safe + lock (lockObject) { + if (graphIndex >= _navmeshHolders.Length) { + var gg = new INavmeshHolder[graphIndex+1]; + _navmeshHolders.CopyTo(gg, 0); + _navmeshHolders = gg; + } + _navmeshHolders[graphIndex] = graph; + } + } + + public static void ClearNavmeshHolder (int graphIndex, INavmeshHolder graph) { + lock (lockObject) { + if (graphIndex < _navmeshHolders.Length && _navmeshHolders[graphIndex] == graph) { + _navmeshHolders[graphIndex] = null; + } + } + } + + /// Set the position of this node to the average of its 3 vertices + public void UpdatePositionFromVertices () { + Int3 a, b, c; + + GetVertices(out a, out b, out c); + position = (a + b + c) * 0.333333f; + } + + /// + /// Return a number identifying a vertex. + /// This number does not necessarily need to be a index in an array but two different vertices (in the same graph) should + /// not have the same vertex numbers. + /// + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public int GetVertexIndex (int i) { + return i == 0 ? v0 : (i == 1 ? v1 : v2); + } + + /// + /// Return a number specifying an index in the source vertex array. + /// The vertex array can for example be contained in a recast tile, or be a navmesh graph, that is graph dependant. + /// This is slower than GetVertexIndex, if you only need to compare vertices, use GetVertexIndex. + /// + public int GetVertexArrayIndex (int i) { + return GetNavmeshHolder(GraphIndex).GetVertexArrayIndex(i == 0 ? v0 : (i == 1 ? v1 : v2)); + } + + /// Returns all 3 vertices of this node in world space + public void GetVertices (out Int3 v0, out Int3 v1, out Int3 v2) { + // Get the object holding the vertex data for this node + // This is usually a graph or a recast graph tile + var holder = GetNavmeshHolder(GraphIndex); + + v0 = holder.GetVertex(this.v0); + v1 = holder.GetVertex(this.v1); + v2 = holder.GetVertex(this.v2); + } + + /// Returns all 3 vertices of this node in graph space + public void GetVerticesInGraphSpace (out Int3 v0, out Int3 v1, out Int3 v2) { + // Get the object holding the vertex data for this node + // This is usually a graph or a recast graph tile + var holder = GetNavmeshHolder(GraphIndex); + + v0 = holder.GetVertexInGraphSpace(this.v0); + v1 = holder.GetVertexInGraphSpace(this.v1); + v2 = holder.GetVertexInGraphSpace(this.v2); + } + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public override Int3 GetVertex (int i) { + return GetNavmeshHolder(GraphIndex).GetVertex(GetVertexIndex(i)); + } + + public Int3 GetVertexInGraphSpace (int i) { + return GetNavmeshHolder(GraphIndex).GetVertexInGraphSpace(GetVertexIndex(i)); + } + + public override int GetVertexCount () { + // A triangle has 3 vertices + return 3; + } + + /// + /// Projects the given point onto the plane of this node's surface. + /// + /// The point will be projected down to a plane that contains the surface of the node. + /// If the point is not contained inside the node, it is projected down onto this plane anyway. + /// + public Vector3 ProjectOnSurface (Vector3 point) { + Int3 a, b, c; + + GetVertices(out a, out b, out c); + var pa = (Vector3)a; + var pb = (Vector3)b; + var pc = (Vector3)c; + var up = Vector3.Cross(pb-pa, pc-pa).normalized; + return point - up * Vector3.Dot(up, point-pa); + } + + public override Vector3 ClosestPointOnNode (Vector3 p) { + Int3 a, b, c; + + GetVertices(out a, out b, out c); + return Pathfinding.Polygon.ClosestPointOnTriangle((float3)(Vector3)a, (float3)(Vector3)b, (float3)(Vector3)c, (float3)p); + } + + /// + /// Closest point on the node when seen from above. + /// This method is mostly for internal use as the methods use it. + /// + /// - The returned point is the closest one on the node to p when seen from above (relative to the graph). + /// This is important mostly for sloped surfaces. + /// - The returned point is an Int3 point in graph space. + /// - It is guaranteed to be inside the node, so if you call with the return value from this method the result is guaranteed to be true. + /// + /// This method is slower than e.g or . + /// However they do not have the same guarantees as this method has. + /// + internal Int3 ClosestPointOnNodeXZInGraphSpace (Vector3 p) { + // Get the vertices that make up the triangle + Int3 a, b, c; + + GetVerticesInGraphSpace(out a, out b, out c); + + // Convert p to graph space + p = GetNavmeshHolder(GraphIndex).transform.InverseTransform(p); + + // Find the closest point on the triangle to p when looking at the triangle from above (relative to the graph) + var closest = Pathfinding.Polygon.ClosestPointOnTriangleXZ((Vector3)a, (Vector3)b, (Vector3)c, p); + + // Make sure the point is actually inside the node + var i3closest = (Int3)closest; + if (ContainsPointInGraphSpace(i3closest)) { + // Common case + return i3closest; + } else { + // Annoying... + // The closest point when converted from floating point coordinates to integer coordinates + // is not actually inside the node. It needs to be inside the node for some methods + // (like for example Linecast) to work properly. + + // Try the 8 integer coordinates around the closest point + // and check if any one of them are completely inside the node. + // This will most likely succeed as it should be very close. + for (int dx = -1; dx <= 1; dx++) { + for (int dz = -1; dz <= 1; dz++) { + if ((dx != 0 || dz != 0)) { + var candidate = new Int3(i3closest.x + dx, i3closest.y, i3closest.z + dz); + if (ContainsPointInGraphSpace(candidate)) return candidate; + } + } + } + + // Happens veery rarely. + // Pick the closest vertex of the triangle. + // The vertex is guaranteed to be inside the triangle. + var da = (a - i3closest).sqrMagnitudeLong; + var db = (b - i3closest).sqrMagnitudeLong; + var dc = (c - i3closest).sqrMagnitudeLong; + return da < db ? (da < dc ? a : c) : (db < dc ? b : c); + } + } + + public override Vector3 ClosestPointOnNodeXZ (Vector3 p) { + // Get all 3 vertices for this node + GetVertices(out Int3 tp1, out Int3 tp2, out Int3 tp3); + return Polygon.ClosestPointOnTriangleXZ((Vector3)tp1, (Vector3)tp2, (Vector3)tp3, p); + } + + /// + /// Checks if point is inside the node when seen from above. + /// + /// Note that is faster than this method as it avoids + /// some coordinate transformations. If you are repeatedly calling this method + /// on many different nodes but with the same point then you should consider + /// transforming the point first and then calling ContainsPointInGraphSpace. + /// + /// + /// Int3 p = (Int3)graph.transform.InverseTransform(point); + /// + /// node.ContainsPointInGraphSpace(p); + /// + /// + public override bool ContainsPoint (Vector3 p) { + return ContainsPointInGraphSpace((Int3)GetNavmeshHolder(GraphIndex).transform.InverseTransform(p)); + } + + /// Checks if point is inside the node when seen from above, as defined by the movement plane + public bool ContainsPoint (Vector3 p, NativeMovementPlane movementPlane) { + // Get all 3 vertices for this node + GetVertices(out var a, out var b, out var c); + var pa = (int3)a; + var pb = (int3)b; + var pc = (int3)c; + var pp = (int3)(Int3)p; + return Polygon.ContainsPoint(ref pa, ref pb, ref pc, ref pp, ref movementPlane); + } + + /// + /// Checks if point is inside the node in graph space. + /// + /// In graph space the up direction is always the Y axis so in principle + /// we project the triangle down on the XZ plane and check if the point is inside the 2D triangle there. + /// + public override bool ContainsPointInGraphSpace (Int3 p) { + // Get all 3 vertices for this node + GetVerticesInGraphSpace(out var a, out var b, out var c); + + if ((long)(b.x - a.x) * (long)(p.z - a.z) - (long)(p.x - a.x) * (long)(b.z - a.z) > 0) return false; + + if ((long)(c.x - b.x) * (long)(p.z - b.z) - (long)(p.x - b.x) * (long)(c.z - b.z) > 0) return false; + + if ((long)(a.x - c.x) * (long)(p.z - c.z) - (long)(p.x - c.x) * (long)(a.z - c.z) > 0) return false; + + return true; + // Equivalent code, but the above code is faster + //return Polygon.IsClockwiseMargin (a,b, p) && Polygon.IsClockwiseMargin (b,c, p) && Polygon.IsClockwiseMargin (c,a, p); + + //return Polygon.ContainsPoint(g.GetVertex(v0),g.GetVertex(v1),g.GetVertex(v2),p); + } + + public static readonly Unity.Profiling.ProfilerMarker MarkerDecode = new Unity.Profiling.ProfilerMarker("Decode"); + public static readonly Unity.Profiling.ProfilerMarker MarkerGetVertices = new Unity.Profiling.ProfilerMarker("GetVertex"); + public static readonly Unity.Profiling.ProfilerMarker MarkerClosest = new Unity.Profiling.ProfilerMarker("MarkerClosest"); + + public override Int3 DecodeVariantPosition (uint pathNodeIndex, uint fractionAlongEdge) { + var edge = (int)(pathNodeIndex - NodeIndex); + var p1 = GetVertex(edge); + var p2 = GetVertex((edge + 1) % 3); + InterpolateEdge(ref p1, ref p2, fractionAlongEdge, out var pos); + return pos; + } + + [BurstCompile(FloatMode = FloatMode.Fast)] + static void InterpolateEdge (ref Int3 p1, ref Int3 p2, uint fractionAlongEdge, out Int3 pos) { + var p = (int3)math.lerp((float3)(int3)p1, (float3)(int3)p2, PathNode.UnQuantizeFractionAlongEdge(fractionAlongEdge)); + pos = new Int3(p.x, p.y, p.z); + } + + public override void OpenAtPoint (Path path, uint pathNodeIndex, Int3 point, uint gScore) { + if (InaccuratePathSearch) { + Open(path, pathNodeIndex, gScore); + } else { + OpenAtPoint(path, pathNodeIndex, point, -1, gScore); + } + } + + public override void Open (Path path, uint pathNodeIndex, uint gScore) { + var pathHandler = (path as IPathInternals).PathHandler; + if (InaccuratePathSearch) { + var pn = pathHandler.pathNodes[pathNodeIndex]; + if (pn.flag1) path.OpenCandidateConnectionsToEndNode(position, pathNodeIndex, NodeIndex, gScore); + + if (connections != null) { + // Iterate over all adjacent nodes + for (int i = connections.Length-1; i >= 0; i--) { + var conn = connections[i]; + var other = conn.node; + if (conn.isOutgoing && other.NodeIndex != pn.parentIndex) { + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, conn.cost + path.GetTraversalCost(other), 0, other.position); + } + } + } + return; + } + // One path node variant is created for each side of the triangle + // This particular path node represents just one of the sides of the triangle. + var edge = (int)(pathNodeIndex - NodeIndex); + OpenAtPoint(path, pathNodeIndex, DecodeVariantPosition(pathNodeIndex, pathHandler.pathNodes[pathNodeIndex].fractionAlongEdge), edge, gScore); + } + + void OpenAtPoint (Path path, uint pathNodeIndex, Int3 pos, int edge, uint gScore) { + var pathHandler = (path as IPathInternals).PathHandler; + var pn = pathHandler.pathNodes[pathNodeIndex]; + if (pn.flag1) path.OpenCandidateConnectionsToEndNode(pos, pathNodeIndex, NodeIndex, gScore); + int visitedEdges = 0; + bool cameFromOtherEdgeInThisTriangle = pn.parentIndex >= NodeIndex && pn.parentIndex < NodeIndex + 3; + + if (connections != null) { + // Iterate over all adjacent nodes + for (int i = connections.Length-1; i >= 0; i--) { + var conn = connections[i]; + if (!conn.isOutgoing) continue; + var other = conn.node; + + // Check if we are moving from a side of this triangle, to the corresponding side on an adjacent triangle. + if (conn.isEdgeShared) { + var sharedEdgeOnOtherNode = conn.adjacentShapeEdge; + var adjacentPathNodeIndex = other.NodeIndex + (uint)sharedEdgeOnOtherNode; + + // Skip checking our parent node. This is purely a performance optimization. + if (adjacentPathNodeIndex == pn.parentIndex) continue; + + if (conn.shapeEdge == edge) { + // Make sure we can traverse the neighbour + if (path.CanTraverse(this, other)) { + var tOther = other as TriangleMeshNode; + + // Fast path out if we know we have already searched this node and we cannot improve it + if (!path.ShouldConsiderPathNode(adjacentPathNodeIndex)) { + continue; + } + + if (conn.edgesAreIdentical) { + // The edge on the other node is identical to this edge (but reversed). + // This means that no other node can reach the other node through that edge. + // This is great, because we can then skip adding that node to the heap just + // to immediatelly pop it again. This is a performance optimization. + + var otherEnteringCost = path.GetTraversalCost(other); + ref var otherPathNode = ref pathHandler.pathNodes[adjacentPathNodeIndex]; + otherPathNode.pathID = path.pathID; + otherPathNode.heapIndex = BinaryHeap.NotInHeap; + otherPathNode.parentIndex = pathNodeIndex; + otherPathNode.fractionAlongEdge = PathNode.ReverseFractionAlongEdge(pn.fractionAlongEdge); + // Make sure the path gets information about us having visited this in-between node, + // even if we never add it to the heap + path.OnVisitNode(adjacentPathNodeIndex, uint.MaxValue, gScore + otherEnteringCost); + pathHandler.LogVisitedNode(adjacentPathNodeIndex, uint.MaxValue, gScore + otherEnteringCost); + + tOther.OpenAtPoint(path, adjacentPathNodeIndex, pos, sharedEdgeOnOtherNode, gScore + otherEnteringCost); + } else { + OpenSingleEdge(path, pathNodeIndex, tOther, sharedEdgeOnOtherNode, pos, gScore); + } + } + } else { + // The other node is a node which shares a different edge with this node. + // We will consider this connection at another time. + + // However, we will consider the move to another side of this triangle, + // namely to the side that *is* shared with the other node. + // If a side of this triangle doesn't share an edge with any connection, we will + // not bother searching it (we will not reach this part of the code), because + // we know its a dead end. + + // If we came from another side of this triangle, it is completely redundant to try to move back to + // another edge in this triangle, because we could always have reached it faster from the parent. + // We also make sure we don't attempt to move to the same edge twice, as that's just a waste of time. + if (!cameFromOtherEdgeInThisTriangle && (visitedEdges & (1 << conn.shapeEdge)) == 0) { + visitedEdges |= 1 << conn.shapeEdge; + OpenSingleEdge(path, pathNodeIndex, this, conn.shapeEdge, pos, gScore); + } + } + } else if (!cameFromOtherEdgeInThisTriangle) { + // This is a connection to some other node type, most likely. For example an off-mesh link. + if (path.CanTraverse(this, other) && path.ShouldConsiderPathNode(other.NodeIndex)) { + var cost = (uint)(other.position - pos).costMagnitude; + + if (edge != -1) { + // We are moving from an edge of this triangle + path.OpenCandidateConnection(pathNodeIndex, other.NodeIndex, gScore, cost, 0, other.position); + } else { + // In some situations we may be moving directly from one off-mesh link to another one without + // passing through any concrete nodes in between. In this case we need to create a temporary node + // to allow the correct path to be reconstructed later. The only important part of the temporary + // node is that we save this node as the associated node. + // This is somewhat ugly, and it limits the number of times we can encounter this case during + // a single search (there's a limit to the number of temporary nodes we can have at the same time). + // Fortunately, this case only happens if there is more than 1 off-mesh link connected to a single + // node, which is quite rare in most games. + // In this case, pathNodeIndex will be another node's index, not a path node belonging to this node. + var viaNode = pathHandler.AddTemporaryNode(new TemporaryNode { + associatedNode = NodeIndex, + position = pos, + targetIndex = 0, + type = TemporaryNodeType.Ignore, + }); + ref var viaPathNode = ref pathHandler.pathNodes[viaNode]; + viaPathNode.pathID = path.pathID; + viaPathNode.parentIndex = pathNodeIndex; + path.OpenCandidateConnection(viaNode, other.NodeIndex, gScore, cost, 0, other.position); + } + } + } + } + } + } + + void OpenSingleEdge (Path path, uint pathNodeIndex, TriangleMeshNode other, int sharedEdgeOnOtherNode, Int3 pos, uint gScore) { + var adjacentPathNodeIndex = other.NodeIndex + (uint)sharedEdgeOnOtherNode; + + // Fast path out if we know we have already searched this node and we cannot improve it + if (!path.ShouldConsiderPathNode(adjacentPathNodeIndex)) { + return; + } + + var s1 = other.GetVertex(sharedEdgeOnOtherNode); + var s2 = other.GetVertex((sharedEdgeOnOtherNode + 1) % 3); + + var pathHandler = (path as IPathInternals).PathHandler; + // TODO: Incorrect, counts nodes multiple times + var otherEnteringCost = path.GetTraversalCost(other); + + var candidateG = gScore + otherEnteringCost; + + OpenSingleEdgeBurst( + ref s1, + ref s2, + ref pos, + path.pathID, + pathNodeIndex, + adjacentPathNodeIndex, + other.NodeIndex, + candidateG, + ref pathHandler.pathNodes, + ref pathHandler.heap, + ref path.heuristicObjectiveInternal + ); + } + + [Unity.Burst.BurstCompile] + static void OpenSingleEdgeBurst (ref Int3 s1, ref Int3 s2, ref Int3 pos, ushort pathID, uint pathNodeIndex, uint candidatePathNodeIndex, uint candidateNodeIndex, uint candidateG, ref UnsafeSpan pathNodes, ref BinaryHeap heap, ref HeuristicObjective heuristicObjective) { + CalculateBestEdgePosition(ref s1, ref s2, ref pos, out var closestPointAlongEdge, out var quantizedFractionAlongEdge, out var cost); + candidateG += cost; + + var pars = new Path.OpenCandidateParams { + pathID = pathID, + parentPathNode = pathNodeIndex, + targetPathNode = candidatePathNodeIndex, + targetNodeIndex = candidateNodeIndex, + candidateG = candidateG, + fractionAlongEdge = quantizedFractionAlongEdge, + targetNodePosition = closestPointAlongEdge, + pathNodes = pathNodes, + }; + Path.OpenCandidateConnectionBurst(ref pars, ref heap, ref heuristicObjective); + } + + [Unity.Burst.BurstCompile] + static void CalculateBestEdgePosition (ref Int3 s1, ref Int3 s2, ref Int3 pos, out int3 closestPointAlongEdge, out uint quantizedFractionAlongEdge, out uint cost) { + // Find the closest point on the other edge. From here on, we will let the position of that path node be this closest point. + // This is much better than using the edge midpoint, and also better than any interpolation between closestFractionAlongEdge + // and the midpoint (0.5). + // In my tests, using the edge midpoint leads to path costs that are rougly 1.3-1.6 times greater than the real distance, + // but using the closest point leads to path costs that are only 1.1-1.2 times greater than the real distance. + // Using triangle centers is the worst option, it leads to path costs that are roughly 1.6-2.0 times greater than the real distance. + // Triangle centers were always used before version 4.3.67. + var v1 = (float3)(int3)s1; + var v2 = (float3)(int3)s2; + var posi = (int3)pos; + var closestFractionAlongEdge = math.clamp(VectorMath.ClosestPointOnLineFactor(v1, v2, (float3)posi), 0, 1); + quantizedFractionAlongEdge = PathNode.QuantizeFractionAlongEdge(closestFractionAlongEdge); + closestFractionAlongEdge = PathNode.UnQuantizeFractionAlongEdge(quantizedFractionAlongEdge); + var closestPointAlongEdgeV = math.lerp(v1, v2, closestFractionAlongEdge); + closestPointAlongEdge = (int3)closestPointAlongEdgeV; + + var diff = posi - closestPointAlongEdge; + cost = (uint)new Int3(diff.x, diff.y, diff.z).costMagnitude; + } + + /// + /// Returns the edge which is shared with other. + /// + /// If there is no shared edge between the two nodes, then -1 is returned. + /// + /// The vertices in the edge can be retrieved using + /// + /// var edge = node.SharedEdge(other); + /// var a = node.GetVertex(edge); + /// var b = node.GetVertex((edge+1) % node.GetVertexCount()); + /// + /// + /// See: which also handles edges that are shared over tile borders and some types of node links + /// + public int SharedEdge (GraphNode other) { + var edge = -1; + + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + if (connections[i].node == other && connections[i].isEdgeShared) edge = connections[i].shapeEdge; + } + } + return edge; + } + + public override bool GetPortal (GraphNode toNode, out Vector3 left, out Vector3 right) { + return GetPortal(toNode, out left, out right, out _, out _); + } + + public bool GetPortalInGraphSpace (TriangleMeshNode toNode, out Int3 a, out Int3 b, out int aIndex, out int bIndex) { + aIndex = -1; + bIndex = -1; + a = Int3.zero; + b = Int3.zero; + + // If the nodes are in different graphs, this function has no idea on how to find a shared edge. + if (toNode.GraphIndex != GraphIndex) return false; + + int edge = -1; + int otherEdge = -1; + if (connections != null) { + for (int i = 0; i < connections.Length; i++) { + if (connections[i].node == toNode && connections[i].isEdgeShared) { + edge = connections[i].shapeEdge; + otherEdge = connections[i].adjacentShapeEdge; + } + } + } + + // -1: No connection was found between the nodes + if (edge == -1) return false; + + aIndex = edge; + bIndex = (edge + 1) % 3; + + // Get the vertices of the shared edge for the first node + var graph = GetNavmeshHolder(GraphIndex); + a = graph.GetVertexInGraphSpace(GetVertexIndex(aIndex)); + b = graph.GetVertexInGraphSpace(GetVertexIndex(bIndex)); + + // Get tiles the nodes are contained in + int tileIndex1 = TileIndex; + int tileIndex2 = toNode.TileIndex; + + if (tileIndex1 != tileIndex2) { + // When the nodes are in different tiles, the edges may not be completely identical + // so another technique is needed. + + // When the nodes are in different tiles, they might not share exactly the same edge + // so we clamp the portal to the segment of the edges which they both have.. + + // Get the vertices of the shared edge for the second node + Int3 v2a = toNode.GetVertexInGraphSpace(otherEdge); + Int3 v2b = toNode.GetVertexInGraphSpace((otherEdge+1) % 3); + graph.GetTileCoordinates(tileIndex1, out var tileX1, out var tileZ1); + graph.GetTileCoordinates(tileIndex2, out var tileX2, out var tileZ2); + var axis = tileX1 == tileX2 ? 0 : 2; + Assert.IsTrue(axis == 0 ? tileX1 == tileX2 : tileZ1 == tileZ2); + // This tile-edge aligned coordinate of the vertices should ideally be identical. + // But somewhere in the pipeline some errors may crop up, and thus they may be off by one. + // TODO: Fix this. + Assert.IsTrue(Mathf.Abs(a[2 - axis] - b[2 - axis]) <= 1); + var mn = Mathf.Min(v2a[axis], v2b[axis]); + var mx = Mathf.Max(v2a[axis], v2b[axis]); + + a[axis] = Mathf.Clamp(a[axis], mn, mx); + b[axis] = Mathf.Clamp(b[axis], mn, mx); + } + + return true; + } + + public bool GetPortal (GraphNode toNode, out Vector3 left, out Vector3 right, out int aIndex, out int bIndex) { + if (toNode is TriangleMeshNode toTriNode && GetPortalInGraphSpace(toTriNode, out var a, out var b, out aIndex, out bIndex)) { + var graph = GetNavmeshHolder(GraphIndex); + // All triangles should be laid out in clockwise order so b is the rightmost vertex (seen from this node) + left = graph.transform.Transform((Vector3)a); + right = graph.transform.Transform((Vector3)b); + return true; + } else { + aIndex = -1; + bIndex = -1; + left = Vector3.zero; + right = Vector3.zero; + return false; + } + } + + /// TODO: This is the area in XZ space, use full 3D space for higher correctness maybe? + public override float SurfaceArea () { + var holder = GetNavmeshHolder(GraphIndex); + + return System.Math.Abs(VectorMath.SignedTriangleAreaTimes2XZ(holder.GetVertex(v0), holder.GetVertex(v1), holder.GetVertex(v2))) * 0.5f; + } + + public override Vector3 RandomPointOnSurface () { + // Find a random point inside the triangle + // This generates uniformly distributed trilinear coordinates + // See http://mathworld.wolfram.com/TrianglePointPicking.html + float2 r; + + do { + r = AstarMath.ThreadSafeRandomFloat2(); + } while (r.x+r.y > 1); + + // Pick the point corresponding to the trilinear coordinate + GetVertices(out var v0, out var v1, out var v2); + return ((Vector3)(v1-v0))*r.x + ((Vector3)(v2-v0))*r.y + (Vector3)v0; + } + + public override void SerializeNode (GraphSerializationContext ctx) { + base.SerializeNode(ctx); + ctx.writer.Write(v0); + ctx.writer.Write(v1); + ctx.writer.Write(v2); + } + + public override void DeserializeNode (GraphSerializationContext ctx) { + base.DeserializeNode(ctx); + v0 = ctx.reader.ReadInt32(); + v1 = ctx.reader.ReadInt32(); + v2 = ctx.reader.ReadInt32(); + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs.meta new file mode 100644 index 0000000..c7a35d6 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Nodes/TriangleMeshNode.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 54908f58720324c048a5b475a27077fa +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point.meta new file mode 100644 index 0000000..b87c4b9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 08766de33bd85be4d9d4b9cfc7b31226 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point/PointKDTree.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point/PointKDTree.cs new file mode 100644 index 0000000..37bc463 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point/PointKDTree.cs @@ -0,0 +1,309 @@ +using System.Collections.Generic; + +namespace Pathfinding { + using Pathfinding.Util; + + /// + /// Represents a collection of GraphNodes. + /// It allows for fast lookups of the closest node to a point. + /// + /// See: https://en.wikipedia.org/wiki/K-d_tree + /// + public class PointKDTree { + public const int LeafSize = 10; + public const int LeafArraySize = LeafSize*2 + 1; + + Node[] tree = new Node[16]; + + int numNodes = 0; + + readonly List largeList = new List(); + readonly Stack arrayCache = new Stack(); + static readonly IComparer[] comparers = new IComparer[] { new CompareX(), new CompareY(), new CompareZ() }; + + struct Node { + /// Nodes in this leaf node (null if not a leaf node) + public GraphNode[] data; + /// Split point along the if not a leaf node + public int split; + /// Number of non-null entries in + public ushort count; + /// Axis to split along if not a leaf node (x=0, y=1, z=2) + public byte splitAxis; + } + + // Pretty ugly with one class for each axis, but it has been verified to make the tree around 5% faster + class CompareX : IComparer { + public int Compare (GraphNode lhs, GraphNode rhs) { return lhs.position.x.CompareTo(rhs.position.x); } + } + + class CompareY : IComparer { + public int Compare (GraphNode lhs, GraphNode rhs) { return lhs.position.y.CompareTo(rhs.position.y); } + } + + class CompareZ : IComparer { + public int Compare (GraphNode lhs, GraphNode rhs) { return lhs.position.z.CompareTo(rhs.position.z); } + } + + public PointKDTree() { + tree[1] = new Node { data = GetOrCreateList() }; + } + + /// Add the node to the tree + public void Add (GraphNode node) { + numNodes++; + Add(node, 1); + } + + /// Rebuild the tree starting with all nodes in the array between index start (inclusive) and end (exclusive) + public void Rebuild (GraphNode[] nodes, int start, int end) { + if (start < 0 || end < start || end > nodes.Length) + throw new System.ArgumentException(); + + for (int i = 0; i < tree.Length; i++) { + var data = tree[i].data; + if (data != null) { + for (int j = 0; j < LeafArraySize; j++) data[j] = null; + arrayCache.Push(data); + tree[i].data = null; + } + } + + numNodes = end - start; + Build(1, new List(nodes), start, end); + } + + GraphNode[] GetOrCreateList () { + // Note, the lists will never become larger than this initial capacity, so possibly they should be replaced by arrays + return arrayCache.Count > 0 ? arrayCache.Pop() : new GraphNode[LeafArraySize]; + } + + int Size (int index) { + return tree[index].data != null ? tree[index].count : Size(2 * index) + Size(2 * index + 1); + } + + void CollectAndClear (int index, List buffer) { + var nodes = tree[index].data; + var count = tree[index].count; + + if (nodes != null) { + tree[index] = new Node(); + for (int i = 0; i < count; i++) { + buffer.Add(nodes[i]); + nodes[i] = null; + } + arrayCache.Push(nodes); + } else { + CollectAndClear(index*2, buffer); + CollectAndClear(index*2 + 1, buffer); + } + } + + static int MaxAllowedSize (int numNodes, int depth) { + // Allow a node to be 2.5 times as full as it should ideally be + // but do not allow it to contain more than 3/4ths of the total number of nodes + // (important to make sure nodes near the top of the tree also get rebalanced). + // A node should ideally contain numNodes/(2^depth) nodes below it (^ is exponentiation, not xor) + return System.Math.Min(((5 * numNodes) / 2) >> depth, (3 * numNodes) / 4); + } + + void Rebalance (int index) { + CollectAndClear(index, largeList); + Build(index, largeList, 0, largeList.Count); + largeList.ClearFast(); + } + + void EnsureSize (int index) { + if (index >= tree.Length) { + var newLeaves = new Node[System.Math.Max(index + 1, tree.Length*2)]; + tree.CopyTo(newLeaves, 0); + tree = newLeaves; + } + } + + void Build (int index, List nodes, int start, int end) { + EnsureSize(index); + if (end - start <= LeafSize) { + var leafData = tree[index].data = GetOrCreateList(); + tree[index].count = (ushort)(end - start); + for (int i = start; i < end; i++) + leafData[i - start] = nodes[i]; + } else { + Int3 mn, mx; + mn = mx = nodes[start].position; + for (int i = start; i < end; i++) { + var p = nodes[i].position; + mn = new Int3(System.Math.Min(mn.x, p.x), System.Math.Min(mn.y, p.y), System.Math.Min(mn.z, p.z)); + mx = new Int3(System.Math.Max(mx.x, p.x), System.Math.Max(mx.y, p.y), System.Math.Max(mx.z, p.z)); + } + Int3 diff = mx - mn; + var axis = diff.x > diff.y ? (diff.x > diff.z ? 0 : 2) : (diff.y > diff.z ? 1 : 2); + + nodes.Sort(start, end - start, comparers[axis]); + int mid = (start+end)/2; + tree[index].split = (nodes[mid-1].position[axis] + nodes[mid].position[axis] + 1)/2; + tree[index].splitAxis = (byte)axis; + Build(index*2 + 0, nodes, start, mid); + Build(index*2 + 1, nodes, mid, end); + } + } + + void Add (GraphNode point, int index, int depth = 0) { + // Move down in the tree until the leaf node is found that this point is inside of + while (tree[index].data == null) { + index = 2 * index + (point.position[tree[index].splitAxis] < tree[index].split ? 0 : 1); + depth++; + } + + // Add the point to the leaf node + tree[index].data[tree[index].count++] = point; + + // Check if the leaf node is large enough that we need to do some rebalancing + if (tree[index].count >= LeafArraySize) { + int levelsUp = 0; + + // Search upwards for nodes that are too large and should be rebalanced + // Rebalance the node above the node that had a too large size so that it can + // move children over to the sibling + while (depth - levelsUp > 0 && Size(index >> levelsUp) > MaxAllowedSize(numNodes, depth-levelsUp)) { + levelsUp++; + } + + Rebalance(index >> levelsUp); + } + } + + /// Closest node to the point which satisfies the constraint and is at most at the given distance + public GraphNode GetNearest (Int3 point, NNConstraint constraint, ref float distanceSqr) { + GraphNode best = null; + long bestSqrDist = distanceSqr < float.PositiveInfinity ? (long)(Int3.FloatPrecision * Int3.FloatPrecision * distanceSqr) : long.MaxValue; + + GetNearestInternal(1, point, constraint, ref best, ref bestSqrDist); + distanceSqr = best != null ? Int3.PrecisionFactor*Int3.PrecisionFactor * bestSqrDist : float.PositiveInfinity; + return best; + } + + void GetNearestInternal (int index, Int3 point, NNConstraint constraint, ref GraphNode best, ref long bestSqrDist) { + var data = tree[index].data; + + if (data != null) { + for (int i = tree[index].count - 1; i >= 0; i--) { + var dist = (data[i].position - point).sqrMagnitudeLong; + if (dist < bestSqrDist && (constraint == null || constraint.Suitable(data[i]))) { + bestSqrDist = dist; + best = data[i]; + } + } + } else { + var dist = (long)(point[tree[index].splitAxis] - tree[index].split); + var childIndex = 2 * index + (dist < 0 ? 0 : 1); + GetNearestInternal(childIndex, point, constraint, ref best, ref bestSqrDist); + + // Try the other one if it is possible to find a valid node on the other side + if (dist*dist < bestSqrDist) { + // childIndex ^ 1 will flip the last bit, so if childIndex is odd, then childIndex ^ 1 will be even + GetNearestInternal(childIndex ^ 0x1, point, constraint, ref best, ref bestSqrDist); + } + } + } + + /// Closest node to the point which satisfies the constraint + public GraphNode GetNearestConnection (Int3 point, NNConstraint constraint, long maximumSqrConnectionLength) { + GraphNode best = null; + long bestSqrDist = long.MaxValue; + + // Given a found point at a distance of r world units + // then any node that has a connection on which a closer point lies must have a squared distance lower than + // d^2 < (maximumConnectionLength/2)^2 + r^2 + // Note: (x/2)^2 = (x^2)/4 + // Note: (x+3)/4 to round up + long offset = (maximumSqrConnectionLength+3)/4; + + GetNearestConnectionInternal(1, point, constraint, ref best, ref bestSqrDist, offset); + return best; + } + + void GetNearestConnectionInternal (int index, Int3 point, NNConstraint constraint, ref GraphNode best, ref long bestSqrDist, long distanceThresholdOffset) { + var data = tree[index].data; + + if (data != null) { + var pointv3 = (UnityEngine.Vector3)point; + for (int i = tree[index].count - 1; i >= 0; i--) { + var dist = (data[i].position - point).sqrMagnitudeLong; + // Note: the subtraction is important. If we used an addition on the RHS instead the result might overflow as bestSqrDist starts as long.MaxValue + if (dist - distanceThresholdOffset < bestSqrDist && (constraint == null || constraint.Suitable(data[i]))) { + // This node may contains the closest connection + // Check all connections + var conns = (data[i] as PointNode).connections; + if (conns != null) { + var nodePos = (UnityEngine.Vector3)data[i].position; + for (int j = 0; j < conns.Length; j++) { + // Find the closest point on the connection, but only on this node's side of the connection + // This ensures that we will find the closest node with the closest connection. + var connectionMidpoint = ((UnityEngine.Vector3)conns[j].node.position + nodePos) * 0.5f; + float sqrConnectionDistance = VectorMath.SqrDistancePointSegment(nodePos, connectionMidpoint, pointv3); + // Convert to Int3 space + long sqrConnectionDistanceInt = (long)(sqrConnectionDistance*Int3.FloatPrecision*Int3.FloatPrecision); + if (sqrConnectionDistanceInt < bestSqrDist) { + bestSqrDist = sqrConnectionDistanceInt; + best = data[i]; + } + } + } + + // Also check if the node itself is close enough. + // This is important if the node has no connections at all. + if (dist < bestSqrDist) { + bestSqrDist = dist; + best = data[i]; + } + } + } + } else { + var dist = (long)(point[tree[index].splitAxis] - tree[index].split); + var childIndex = 2 * index + (dist < 0 ? 0 : 1); + GetNearestConnectionInternal(childIndex, point, constraint, ref best, ref bestSqrDist, distanceThresholdOffset); + + // Try the other one if it is possible to find a valid node on the other side + // Note: the subtraction is important. If we used an addition on the RHS instead the result might overflow as bestSqrDist starts as long.MaxValue + if (dist*dist - distanceThresholdOffset < bestSqrDist) { + // childIndex ^ 1 will flip the last bit, so if childIndex is odd, then childIndex ^ 1 will be even + GetNearestConnectionInternal(childIndex ^ 0x1, point, constraint, ref best, ref bestSqrDist, distanceThresholdOffset); + } + } + } + + /// Add all nodes within a squared distance of the point to the buffer. + /// Nodes around this point will be added to the buffer. + /// squared maximum distance in Int3 space. If you are converting from world space you will need to multiply by Int3.Precision: + /// var sqrRadius = (worldSpaceRadius * Int3.Precision) * (worldSpaceRadius * Int3.Precision); + /// All nodes will be added to this list. + public void GetInRange (Int3 point, long sqrRadius, List buffer) { + GetInRangeInternal(1, point, sqrRadius, buffer); + } + + void GetInRangeInternal (int index, Int3 point, long sqrRadius, List buffer) { + var data = tree[index].data; + + if (data != null) { + for (int i = tree[index].count - 1; i >= 0; i--) { + var dist = (data[i].position - point).sqrMagnitudeLong; + if (dist < sqrRadius) { + buffer.Add(data[i]); + } + } + } else { + var dist = (long)(point[tree[index].splitAxis] - tree[index].split); + // Pick the first child to enter based on which side of the splitting line the point is + var childIndex = 2 * index + (dist < 0 ? 0 : 1); + GetInRangeInternal(childIndex, point, sqrRadius, buffer); + + // Try the other one if it is possible to find a valid node on the other side + if (dist*dist < sqrRadius) { + // childIndex ^ 1 will flip the last bit, so if childIndex is odd, then childIndex ^ 1 will be even + GetInRangeInternal(childIndex ^ 0x1, point, sqrRadius, buffer); + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point/PointKDTree.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point/PointKDTree.cs.meta new file mode 100644 index 0000000..25bfb88 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Point/PointKDTree.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 4aef007a0dd474c20872caa35fbbc8a7 +timeCreated: 1462714767 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/PointGraph.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/PointGraph.cs new file mode 100644 index 0000000..b99b874 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/PointGraph.cs @@ -0,0 +1,772 @@ +using UnityEngine; +using System.Collections.Generic; +using Pathfinding.Serialization; +using Pathfinding.Util; + +namespace Pathfinding { + using Pathfinding.Drawing; + using Unity.Jobs; + + /// + /// Basic point graph. + /// + /// The point graph is the most basic graph structure, it consists of a number of interconnected points in space called nodes or waypoints. + /// The point graph takes a Transform object as "root", this Transform will be searched for child objects, every child object will be treated as a node. + /// If is enabled, it will also search the child objects of the children recursively. + /// It will then check if any connections between the nodes can be made, first it will check if the distance between the nodes isn't too large ( + /// and then it will check if the axis aligned distance isn't too high. The axis aligned distance, named , + /// is useful because usually an AI cannot climb very high, but linking nodes far away from each other, + /// but on the same Y level should still be possible. and are treated as being set to infinity if they are set to 0 (zero). + /// Lastly it will check if there are any obstructions between the nodes using + /// raycasting which can optionally be thick. + /// One thing to think about when using raycasting is to either place the nodes a small + /// distance above the ground in your scene or to make sure that the ground is not in the raycast mask to avoid the raycast from hitting the ground. + /// + /// Alternatively, a tag can be used to search for nodes. + /// See: http://docs.unity3d.com/Manual/Tags.html + /// + /// For larger graphs, it can take quite some time to scan the graph with the default settings. + /// You can enable which will in most cases reduce the calculation times drastically. + /// + /// Note: Does not support linecast because of obvious reasons. + /// + /// [Open online documentation to see images] + /// [Open online documentation to see images] + /// + [JsonOptIn] + [Pathfinding.Util.Preserve] + public class PointGraph : NavGraph + , IUpdatableGraph { + /// Childs of this transform are treated as nodes + [JsonMember] + public Transform root; + + /// If no is set, all nodes with the tag is used as nodes + [JsonMember] + public string searchTag; + + /// + /// Max distance for a connection to be valid. + /// The value 0 (zero) will be read as infinity and thus all nodes not restricted by + /// other constraints will be added as connections. + /// + /// A negative value will disable any neighbours to be added. + /// It will completely stop the connection processing to be done, so it can save you processing + /// power if you don't these connections. + /// + [JsonMember] + public float maxDistance; + + /// Max distance along the axis for a connection to be valid. 0 = infinity + [JsonMember] + public Vector3 limits; + + /// Use raycasts to check connections + [JsonMember] + public bool raycast = true; + + /// Use the 2D Physics API + [JsonMember] + public bool use2DPhysics; + + /// Use thick raycast + [JsonMember] + public bool thickRaycast; + + /// Thick raycast radius + [JsonMember] + public float thickRaycastRadius = 1; + + /// Recursively search for child nodes to the + [JsonMember] + public bool recursive = true; + + /// Layer mask to use for raycast + [JsonMember] + public LayerMask mask; + + /// + /// Optimizes the graph for sparse graphs. + /// + /// This can reduce calculation times for both scanning and for normal path requests by huge amounts. + /// It reduces the number of node-node checks that need to be done during scan, and can also optimize getting the nearest node from the graph (such as when querying for a path). + /// + /// Try enabling and disabling this option, check the scan times logged when you scan the graph to see if your graph is suited for this optimization + /// or if it makes it slower. + /// + /// The gain of using this optimization increases with larger graphs, the default scan algorithm is brute force and requires O(n^2) checks, this optimization + /// along with a graph suited for it, requires only O(n) checks during scan (assuming the connection distance limits are reasonable). + /// + /// Warning: + /// When you have this enabled, you will not be able to move nodes around using scripting unless you recalculate the lookup structure at the same time. + /// See: + /// + /// If you enable this during runtime, you will need to call to make sure any existing nodes are added to the lookup structure. + /// If the graph doesn't have any nodes yet or if you are going to scan the graph afterwards then you do not need to do this. + /// + [JsonMember] + public bool optimizeForSparseGraph; + + PointKDTree lookupTree = new PointKDTree(); + + /// + /// Longest known connection. + /// In squared Int3 units. + /// + /// See: + /// + long maximumConnectionLength = 0; + + /// + /// All nodes in this graph. + /// Note that only the first will be non-null. + /// + /// You can also use the GetNodes method to get all nodes. + /// + public PointNode[] nodes; + + /// + /// \copydoc Pathfinding::PointGraph::NodeDistanceMode + /// + /// See: + /// + /// If you enable this during runtime, you will need to call to make sure some cache data is properly recalculated. + /// If the graph doesn't have any nodes yet or if you are going to scan the graph afterwards then you do not need to do this. + /// + [JsonMember] + public NodeDistanceMode nearestNodeDistanceMode; + + /// Number of nodes in this graph + public int nodeCount { get; protected set; } + + public override bool isScanned => nodes != null; + + /// + /// Distance query mode. + /// [Open online documentation to see images] + /// + /// In the image above there are a few red nodes. Assume the agent is the orange circle. Using the Node mode the closest point on the graph that would be found would be the node at the bottom center which + /// may not be what you want. Using the Connection mode it will find the closest point on the connection between the two nodes in the top half of the image. + /// + /// When using the Connection option you may also want to use the Connection option for the Seeker's Start End Modifier snapping options. + /// This is not strictly necessary, but it most cases it is what you want. + /// + /// See: + /// + public enum NodeDistanceMode { + /// + /// All nearest node queries find the closest node center. + /// This is the fastest option but it may not be what you want if you have long connections. + /// + Node, + /// + /// All nearest node queries find the closest point on edges between nodes. + /// This is useful if you have long connections where the agent might be closer to some unrelated node if it is standing on a long connection between two nodes. + /// This mode is however slower than the Node mode. + /// + Connection, + } + + public override int CountNodes () { + return nodeCount; + } + + public override void GetNodes (System.Action action) { + if (nodes == null) return; + var count = nodeCount; + for (int i = 0; i < count; i++) action(nodes[i]); + } + + public override NNInfo GetNearest (Vector3 position, NNConstraint constraint, float maxDistanceSqr) { + if (nodes == null) return NNInfo.Empty; + var iposition = (Int3)position; + + if (optimizeForSparseGraph) { + if (nearestNodeDistanceMode == NodeDistanceMode.Node) { + var minDistSqr = maxDistanceSqr; + var closestNode = lookupTree.GetNearest(iposition, constraint, ref minDistSqr); + return new NNInfo(closestNode, (Vector3)closestNode.position, minDistSqr); + } else { + var closestNode = lookupTree.GetNearestConnection(iposition, constraint, maximumConnectionLength); + if (closestNode == null) return NNInfo.Empty; + + return FindClosestConnectionPoint(closestNode as PointNode, position, maxDistanceSqr); + } + } + + PointNode minNode = null; + long minDist = AstarMath.SaturatingConvertFloatToLong(maxDistanceSqr * Int3.FloatPrecision * Int3.FloatPrecision); + + for (int i = 0; i < nodeCount; i++) { + PointNode node = nodes[i]; + long dist = (iposition - node.position).sqrMagnitudeLong; + + if (dist < minDist && (constraint == null || constraint.Suitable(node))) { + minDist = dist; + minNode = node; + } + } + + float distSqr = Int3.PrecisionFactor*Int3.PrecisionFactor*minDist; + // Do a final distance check here just to make sure we don't exceed the max distance due to rounding errors when converting between longs and floats + return distSqr < maxDistanceSqr && minNode != null ? new NNInfo(minNode, (Vector3)minNode.position, Int3.PrecisionFactor*Int3.PrecisionFactor*minDist) : NNInfo.Empty; + } + + NNInfo FindClosestConnectionPoint (PointNode node, Vector3 position, float maxDistanceSqr) { + var closestConnectionPoint = (Vector3)node.position; + var conns = node.connections; + var nodePos = (Vector3)node.position; + + if (conns != null) { + for (int i = 0; i < conns.Length; i++) { + var connectionMidpoint = ((UnityEngine.Vector3)conns[i].node.position + nodePos) * 0.5f; + var closestPoint = VectorMath.ClosestPointOnSegment(nodePos, connectionMidpoint, position); + var dist = (closestPoint - position).sqrMagnitude; + if (dist < maxDistanceSqr) { + maxDistanceSqr = dist; + closestConnectionPoint = closestPoint; + } + } + } + + return new NNInfo(node, closestConnectionPoint, maxDistanceSqr); + } + + /// + /// Add a node to the graph at the specified position. + /// Note: Vector3 can be casted to Int3 using (Int3)myVector. + /// + /// Note: This needs to be called when it is safe to update nodes, which is + /// - when scanning + /// - during a graph update + /// - inside a callback registered using AstarPath.AddWorkItem + /// + /// + /// AstarPath.active.AddWorkItem(new AstarWorkItem(ctx => { + /// var graph = AstarPath.active.data.pointGraph; + /// // Add 2 nodes and connect them + /// var node1 = graph.AddNode((Int3)transform.position); + /// var node2 = graph.AddNode((Int3)(transform.position + Vector3.right)); + /// var cost = (uint)(node2.position - node1.position).costMagnitude; + /// GraphNode.Connect(node1, node2, cost); + /// })); + /// + /// + /// See: runtime-graphs (view in online documentation for working links) + /// + public PointNode AddNode (Int3 position) { + return AddNode(new PointNode(active), position); + } + + /// + /// Add a node with the specified type to the graph at the specified position. + /// + /// Note: Vector3 can be casted to Int3 using (Int3)myVector. + /// + /// Note: This needs to be called when it is safe to update nodes, which is + /// - when scanning + /// - during a graph update + /// - inside a callback registered using AstarPath.AddWorkItem + /// + /// See: + /// See: runtime-graphs (view in online documentation for working links) + /// + /// This must be a node created using T(AstarPath.active) right before the call to this method. + /// The node parameter is only there because there is no new(AstarPath) constraint on + /// generic type parameters. + /// The node will be set to this position. + public T AddNode(T node, Int3 position) where T : PointNode { + AssertSafeToUpdateGraph(); + if (nodes == null || nodeCount == nodes.Length) { + var newNodes = new PointNode[nodes != null ? System.Math.Max(nodes.Length+4, nodes.Length*2) : 4]; + if (nodes != null) nodes.CopyTo(newNodes, 0); + nodes = newNodes; + } + + node.SetPosition(position); + node.GraphIndex = graphIndex; + node.Walkable = true; + + nodes[nodeCount] = node; + nodeCount++; + + if (optimizeForSparseGraph) AddToLookup(node); + + return node; + } + + /// Recursively counds children of a transform + protected static int CountChildren (Transform tr) { + int c = 0; + + foreach (Transform child in tr) { + c++; + c += CountChildren(child); + } + return c; + } + + /// Recursively adds childrens of a transform as nodes + protected static void AddChildren (PointNode[] nodes, ref int c, Transform tr) { + foreach (Transform child in tr) { + nodes[c].position = (Int3)child.position; + nodes[c].Walkable = true; + nodes[c].gameObject = child.gameObject; + + c++; + AddChildren(nodes, ref c, child); + } + } + + /// + /// Rebuilds the lookup structure for nodes. + /// + /// This is used when is enabled. + /// + /// You should call this method every time you move a node in the graph manually and + /// you are using , otherwise pathfinding might not work correctly. + /// + /// You may also call this after you have added many nodes using the + /// method. When adding nodes using the method they + /// will be added to the lookup structure. The lookup structure will + /// rebalance itself when it gets too unbalanced however if you are + /// sure you won't be adding any more nodes in the short term, you can + /// make sure it is perfectly balanced and thus squeeze out the last + /// bit of performance by calling this method. This can improve the + /// performance of the method slightly. The improvements + /// are on the order of 10-20%. + /// + public void RebuildNodeLookup () { + lookupTree = BuildNodeLookup(nodes, nodeCount, optimizeForSparseGraph); + RebuildConnectionDistanceLookup(); + } + + static PointKDTree BuildNodeLookup (PointNode[] nodes, int nodeCount, bool optimizeForSparseGraph) { + if (optimizeForSparseGraph && nodes != null) { + var lookupTree = new PointKDTree(); + lookupTree.Rebuild(nodes, 0, nodeCount); + return lookupTree; + } else { + return null; + } + } + + /// Rebuilds a cache used when = .ToConnection + public void RebuildConnectionDistanceLookup () { + if (nearestNodeDistanceMode == NodeDistanceMode.Connection) { + maximumConnectionLength = LongestConnectionLength(nodes, nodeCount); + } else { + maximumConnectionLength = 0; + } + } + + static long LongestConnectionLength (PointNode[] nodes, int nodeCount) { + long maximumConnectionLength = 0; + for (int j = 0; j < nodeCount; j++) { + var node = nodes[j]; + var conns = node.connections; + if (conns != null) { + for (int i = 0; i < conns.Length; i++) { + var distSqr = (node.position - conns[i].node.position).sqrMagnitudeLong; + maximumConnectionLength = System.Math.Max(maximumConnectionLength, distSqr); + } + } + } + return maximumConnectionLength; + } + + void AddToLookup (PointNode node) { + lookupTree.Add(node); + } + + /// + /// Ensures the graph knows that there is a connection with this length. + /// This is used when the nearest node distance mode is set to ToConnection. + /// If you are modifying node connections yourself (i.e. manipulating the PointNode.connections array) then you must call this function + /// when you add any connections. + /// + /// When using GraphNode.Connect this is done automatically. + /// It is also done for all nodes when is called. + /// + /// The length of the connection in squared Int3 units. This can be calculated using (node1.position - node2.position).sqrMagnitudeLong. + public void RegisterConnectionLength (long sqrLength) { + maximumConnectionLength = System.Math.Max(maximumConnectionLength, sqrLength); + } + + protected virtual PointNode[] CreateNodes (int count) { + var nodes = new PointNode[count]; + + for (int i = 0; i < count; i++) nodes[i] = new PointNode(active); + return nodes; + } + + class PointGraphScanPromise : IGraphUpdatePromise { + public PointGraph graph; + PointKDTree lookupTree; + PointNode[] nodes; + + public IEnumerator Prepare () { + var root = graph.root; + if (root == null) { + // If there is no root object, try to find nodes with the specified tag instead + GameObject[] gos = graph.searchTag != null? GameObject.FindGameObjectsWithTag(graph.searchTag) : null; + + if (gos == null) { + nodes = new PointNode[0]; + } else { + // Create all the nodes + nodes = graph.CreateNodes(gos.Length); + + for (int i = 0; i < gos.Length; i++) { + var node = nodes[i]; + node.position = (Int3)gos[i].transform.position; + node.Walkable = true; + node.gameObject = gos[i].gameObject; + } + } + } else { + // Search the root for children and create nodes for them + if (!graph.recursive) { + var nodeCount = root.childCount; + nodes = graph.CreateNodes(nodeCount); + + int c = 0; + foreach (Transform child in root) { + var node = nodes[c]; + node.position = (Int3)child.position; + node.Walkable = true; + node.gameObject = child.gameObject; + c++; + } + } else { + var nodeCount = CountChildren(root); + nodes = graph.CreateNodes(nodeCount); + + int nodeIndex = 0; + AddChildren(nodes, ref nodeIndex, root); + UnityEngine.Assertions.Assert.AreEqual(nodeIndex, nodeCount); + } + } + + yield return default; + lookupTree = BuildNodeLookup(nodes, nodes.Length, graph.optimizeForSparseGraph); + + foreach (var progress in ConnectNodesAsync(nodes, nodes.Length, lookupTree, graph.maxDistance, graph.limits, graph)) yield return default; + } + + public void Apply (IGraphUpdateContext ctx) { + // Destroy all previous nodes (if any) + graph.DestroyAllNodes(); + // Assign the new node data + graph.lookupTree = lookupTree; + graph.nodes = nodes; + graph.nodeCount = nodes.Length; + graph.maximumConnectionLength = graph.nearestNodeDistanceMode == NodeDistanceMode.Connection ? LongestConnectionLength(nodes, nodes.Length) : 0; + } + } + + protected override IGraphUpdatePromise ScanInternal () => new PointGraphScanPromise { graph = this }; + + /// + /// Recalculates connections for all nodes in the graph. + /// This is useful if you have created nodes manually using and then want to connect them in the same way as the point graph normally connects nodes. + /// + public void ConnectNodes () { + AssertSafeToUpdateGraph(); + var ie = ConnectNodesAsync(nodes, nodeCount, lookupTree, maxDistance, limits, this).GetEnumerator(); + + while (ie.MoveNext()) {} + + RebuildConnectionDistanceLookup(); + } + + /// + /// Calculates connections for all nodes in the graph. + /// This is an IEnumerable, you can iterate through it using e.g foreach to get progress information. + /// + static IEnumerable ConnectNodesAsync (PointNode[] nodes, int nodeCount, PointKDTree lookupTree, float maxDistance, Vector3 limits, PointGraph graph) { + if (maxDistance >= 0) { + // To avoid too many allocations, these lists are reused for each node + var connections = new List(); + var candidateConnections = new List(); + + long maxSquaredRange; + // Max possible squared length of a connection between two nodes + // This is used to speed up the calculations by skipping a lot of nodes that do not need to be checked + if (maxDistance == 0 && (limits.x == 0 || limits.y == 0 || limits.z == 0)) { + maxSquaredRange = long.MaxValue; + } else { + maxSquaredRange = (long)(Mathf.Max(limits.x, Mathf.Max(limits.y, Mathf.Max(limits.z, maxDistance))) * Int3.Precision) + 1; + maxSquaredRange *= maxSquaredRange; + } + + // Report progress every N nodes + const int YieldEveryNNodes = 512; + + // Loop through all nodes and add connections to other nodes + for (int i = 0; i < nodeCount; i++) { + if (i % YieldEveryNNodes == 0) { + yield return i/(float)nodeCount; + } + + connections.Clear(); + var node = nodes[i]; + if (lookupTree != null) { + candidateConnections.Clear(); + lookupTree.GetInRange(node.position, maxSquaredRange, candidateConnections); + for (int j = 0; j < candidateConnections.Count; j++) { + var other = candidateConnections[j] as PointNode; + if (other != node && graph.IsValidConnection(node, other, out var dist)) { + connections.Add(new Connection( + other, + /// TODO: Is this equal to .costMagnitude + (uint)Mathf.RoundToInt(dist*Int3.FloatPrecision), + true, + true + )); + } + } + } else { + // brute force + for (int j = 0; j < nodeCount; j++) { + if (i == j) continue; + + PointNode other = nodes[j]; + if (graph.IsValidConnection(node, other, out var dist)) { + connections.Add(new Connection( + other, + /// TODO: Is this equal to .costMagnitude + (uint)Mathf.RoundToInt(dist*Int3.FloatPrecision), + true, + true + )); + } + } + } + node.connections = connections.ToArray(); + node.SetConnectivityDirty(); + } + } + } + + /// + /// Returns if the connection between a and b is valid. + /// Checks for obstructions using raycasts (if enabled) and checks for height differences. + /// As a bonus, it outputs the distance between the nodes too if the connection is valid. + /// + /// Note: This is not the same as checking if node a is connected to node b. + /// That should be done using a.ContainsOutgoingConnection(b) + /// + public virtual bool IsValidConnection (GraphNode a, GraphNode b, out float dist) { + dist = 0; + + if (!a.Walkable || !b.Walkable) return false; + + var dir = (Vector3)(b.position-a.position); + + if ( + (!Mathf.Approximately(limits.x, 0) && Mathf.Abs(dir.x) > limits.x) || + (!Mathf.Approximately(limits.y, 0) && Mathf.Abs(dir.y) > limits.y) || + (!Mathf.Approximately(limits.z, 0) && Mathf.Abs(dir.z) > limits.z)) { + return false; + } + + dist = dir.magnitude; + if (maxDistance == 0 || dist < maxDistance) { + if (raycast) { + var ray = new Ray((Vector3)a.position, dir); + var invertRay = new Ray((Vector3)b.position, -dir); + + if (use2DPhysics) { + if (thickRaycast) { + return !Physics2D.CircleCast(ray.origin, thickRaycastRadius, ray.direction, dist, mask) && !Physics2D.CircleCast(invertRay.origin, thickRaycastRadius, invertRay.direction, dist, mask); + } else { + return !Physics2D.Linecast((Vector2)(Vector3)a.position, (Vector2)(Vector3)b.position, mask) && !Physics2D.Linecast((Vector2)(Vector3)b.position, (Vector2)(Vector3)a.position, mask); + } + } else { + if (thickRaycast) { + return !Physics.SphereCast(ray, thickRaycastRadius, dist, mask) && !Physics.SphereCast(invertRay, thickRaycastRadius, dist, mask); + } else { + return !Physics.Linecast((Vector3)a.position, (Vector3)b.position, mask) && !Physics.Linecast((Vector3)b.position, (Vector3)a.position, mask); + } + } + } else { + return true; + } + } + return false; + } + + class PointGraphUpdatePromise : IGraphUpdatePromise { + public PointGraph graph; + public List graphUpdates; + + public void Apply (IGraphUpdateContext ctx) { + var nodes = graph.nodes; + for (int u = 0; u < graphUpdates.Count; u++) { + var guo = graphUpdates[u]; + for (int i = 0; i < graph.nodeCount; i++) { + var node = nodes[i]; + if (guo.bounds.Contains((Vector3)node.position)) { + guo.WillUpdateNode(node); + guo.Apply(node); + } + } + + if (guo.updatePhysics) { + // Use a copy of the bounding box, we should not change the GUO's bounding box since it might be used for other graph updates + Bounds bounds = guo.bounds; + + if (graph.thickRaycast) { + // Expand the bounding box to account for the thick raycast + bounds.Expand(graph.thickRaycastRadius*2); + } + + // Create a temporary list used for holding connection data + List tmpList = Pathfinding.Util.ListPool.Claim(); + + for (int i = 0; i < graph.nodeCount; i++) { + PointNode node = graph.nodes[i]; + var nodePos = (Vector3)node.position; + + List conn = null; + + for (int j = 0; j < graph.nodeCount; j++) { + if (j == i) continue; + + var otherNodePos = (Vector3)nodes[j].position; + // Check if this connection intersects the bounding box. + // If it does we need to recalculate that connection. + if (VectorMath.SegmentIntersectsBounds(bounds, nodePos, otherNodePos)) { + float dist; + PointNode other = nodes[j]; + bool contains = node.ContainsOutgoingConnection(other); + bool validConnection = graph.IsValidConnection(node, other, out dist); + + // Fill the 'conn' list when we need to change a connection + if (conn == null && (contains != validConnection)) { + tmpList.Clear(); + conn = tmpList; + conn.AddRange(node.connections); + } + + if (!contains && validConnection) { + // A new connection should be added + uint cost = (uint)Mathf.RoundToInt(dist*Int3.FloatPrecision); + conn.Add(new Connection(other, cost, true, true)); + graph.RegisterConnectionLength((other.position - node.position).sqrMagnitudeLong); + } else if (contains && !validConnection) { + // A connection should be removed + for (int q = 0; q < conn.Count; q++) { + if (conn[q].node == other) { + conn.RemoveAt(q); + break; + } + } + } + } + } + + // Save the new connections if any were changed + if (conn != null) { + node.connections = conn.ToArray(); + node.SetConnectivityDirty(); + } + } + + // Release buffers back to the pool + ListPool.Release(ref tmpList); + ctx.DirtyBounds(guo.bounds); + } + } + + ListPool.Release(ref graphUpdates); + } + } + + /// + /// Updates an area in the list graph. + /// Recalculates possibly affected connections, i.e all connectionlines passing trough the bounds of the guo will be recalculated + /// + IGraphUpdatePromise IUpdatableGraph.ScheduleGraphUpdates (List graphUpdates) { + if (!isScanned) return null; + + return new PointGraphUpdatePromise { + graph = this, + graphUpdates = graphUpdates + }; + } + +#if UNITY_EDITOR + public override void OnDrawGizmos (DrawingData gizmos, bool drawNodes, RedrawScope redrawScope) { + base.OnDrawGizmos(gizmos, drawNodes, redrawScope); + + if (!drawNodes) return; + + using (var draw = gizmos.GetBuilder()) { + using (draw.WithColor(new Color(0.161f, 0.341f, 1f, 0.5f))) { + if (root != null) { + DrawChildren(draw, this, root); + } else if (!string.IsNullOrEmpty(searchTag)) { + GameObject[] gos = GameObject.FindGameObjectsWithTag(searchTag); + for (int i = 0; i < gos.Length; i++) { + draw.SolidBox(gos[i].transform.position, Vector3.one*UnityEditor.HandleUtility.GetHandleSize(gos[i].transform.position)*0.1F); + } + } + } + } + } + + static void DrawChildren (CommandBuilder draw, PointGraph graph, Transform tr) { + foreach (Transform child in tr) { + draw.SolidBox(child.position, Vector3.one*UnityEditor.HandleUtility.GetHandleSize(child.position)*0.1F); + if (graph.recursive) DrawChildren(draw, graph, child); + } + } +#endif + + protected override void PostDeserialization (GraphSerializationContext ctx) { + RebuildNodeLookup(); + } + + public override void RelocateNodes (Matrix4x4 deltaMatrix) { + base.RelocateNodes(deltaMatrix); + RebuildNodeLookup(); + } + + protected override void SerializeExtraInfo (GraphSerializationContext ctx) { + // Serialize node data + + if (nodes == null) ctx.writer.Write(-1); + + // Length prefixed array of nodes + ctx.writer.Write(nodeCount); + for (int i = 0; i < nodeCount; i++) { + // -1 indicates a null field + if (nodes[i] == null) ctx.writer.Write(-1); + else { + ctx.writer.Write(0); + nodes[i].SerializeNode(ctx); + } + } + } + + protected override void DeserializeExtraInfo (GraphSerializationContext ctx) { + int count = ctx.reader.ReadInt32(); + + if (count == -1) { + nodes = null; + return; + } + + nodes = new PointNode[count]; + nodeCount = count; + + for (int i = 0; i < nodes.Length; i++) { + if (ctx.reader.ReadInt32() == -1) continue; + nodes[i] = new PointNode(active); + nodes[i].DeserializeNode(ctx); + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/PointGraph.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/PointGraph.cs.meta new file mode 100644 index 0000000..03cc34a --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/PointGraph.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: f7458b5480c614cebb219a8f7f5df111 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/RecastGraph.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/RecastGraph.cs new file mode 100644 index 0000000..fb0ea6d --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/RecastGraph.cs @@ -0,0 +1,1392 @@ +using UnityEngine; +using System.Collections.Generic; +using UnityEngine.Profiling; +using Unity.Jobs; +using Unity.Mathematics; + +namespace Pathfinding { + using Pathfinding.Serialization; + using Pathfinding.Graphs.Navmesh; + using Pathfinding.Util; + using Pathfinding.Jobs; + using Pathfinding.Graphs.Navmesh.Jobs; + + /// + /// Automatically generates navmesh graphs based on world geometry. + /// + /// [Open online documentation to see images] + /// + /// The recast graph is based on Recast (http://code.google.com/p/recastnavigation/). + /// I have translated a good portion of it to C# to run it natively in Unity. + /// + /// For a tutorial on how to configure a recast graph, take a look at create-recast (view in online documentation for working links). + /// + /// [Open online documentation to see images] + /// + /// \section howitworks How a recast graph works + /// When generating a recast graph what happens is that the world is voxelized. + /// You can think of this as constructing an approximation of the world out of lots of boxes. + /// If you have played Minecraft it looks very similar (but with smaller boxes). + /// [Open online documentation to see images] + /// + /// The Recast process is described as follows: + /// - The voxel mold is build from the input triangle mesh by rasterizing the triangles into a multi-layer heightfield. + /// Some simple filters are then applied to the mold to prune out locations where the character would not be able to move. + /// - The walkable areas described by the mold are divided into simple overlayed 2D regions. + /// The resulting regions have only one non-overlapping contour, which simplifies the final step of the process tremendously. + /// - The navigation polygons are peeled off from the regions by first tracing the boundaries and then simplifying them. + /// The resulting polygons are finally converted to triangles which makes them perfect for pathfinding and spatial reasoning about the level. + /// + /// The recast generation process usually works directly on the visiable geometry in the world. This is usually a good thing, because world geometry is usually more detailed than the colliders. + /// You can, however, specify that colliders should be rasterized instead. If you have very detailed world geometry, this can speed up scanning and updating the graph. + /// + /// \section export Exporting for manual editing + /// In the editor there is a button for exporting the generated graph to a .obj file. + /// Usually the generation process is good enough for the game directly, but in some cases you might want to edit some minor details. + /// So you can export the graph to a .obj file, open it in your favourite 3D application, edit it, and export it to a mesh which Unity can import. + /// You can then use that mesh in a navmesh graph. + /// + /// Since many 3D modelling programs use different axis systems (unity uses X=right, Y=up, Z=forward), it can be a bit tricky to get the rotation and scaling right. + /// For blender for example, what you have to do is to first import the mesh using the .obj importer. Don't change anything related to axes in the settings. + /// Then select the mesh, open the transform tab (usually the thin toolbar to the right of the 3D view) and set Scale -> Z to -1. + /// If you transform it using the S (scale) hotkey, it seems to set both Z and Y to -1 for some reason. + /// Then make the edits you need and export it as an .obj file to somewhere in the Unity project. + /// But this time, edit the setting named "Forward" to "Z forward" (not -Z as it is per default). + /// + [JsonOptIn] + [Pathfinding.Util.Preserve] + public class RecastGraph : NavmeshBase, IUpdatableGraph { + [JsonMember] + /// + /// Radius of the agent which will traverse the navmesh. + /// The navmesh will be eroded with this radius. + /// [Open online documentation to see images] + /// + public float characterRadius = 1.5F; + + /// + /// Max distance from simplified edge to real edge. + /// This value is measured in voxels. So with the default value of 2 it means that the final navmesh contour may be at most + /// 2 voxels (i.e 2 times away from the border that was calculated when voxelizing the world. + /// A higher value will yield a more simplified and cleaner navmesh while a lower value may capture more details. + /// However a too low value will cause the individual voxels to be visible (see image below). + /// + /// [Open online documentation to see images] + /// + /// See: + /// + [JsonMember] + public float contourMaxError = 2F; + + /// + /// Voxel sample size (x,z). + /// When generating a recast graph what happens is that the world is voxelized. + /// You can think of this as constructing an approximation of the world out of lots of boxes. + /// If you have played Minecraft it looks very similar (but with smaller boxes). + /// [Open online documentation to see images] + /// The cell size is the width and depth of those boxes. The height of the boxes is usually much smaller + /// and automatically calculated, however. + /// + /// Lower values will yield higher quality navmeshes, however the graph will be slower to scan. + /// + /// [Open online documentation to see images] + /// + [JsonMember] + public float cellSize = 0.5F; + + /// + /// Character height. + /// [Open online documentation to see images] + /// + [JsonMember] + public float walkableHeight = 2F; + + /// + /// Height the character can climb. + /// [Open online documentation to see images] + /// + [JsonMember] + public float walkableClimb = 0.5F; + + /// + /// Max slope in degrees the character can traverse. + /// [Open online documentation to see images] + /// + [JsonMember] + public float maxSlope = 30; + + /// + /// Longer edges will be subdivided. + /// Reducing this value can sometimes improve path quality since similarly sized triangles + /// yield better paths than really large and really triangles small next to each other. + /// However it will also add a lot more nodes which will make pathfinding slower. + /// For more information about this take a look at navmeshnotes (view in online documentation for working links). + /// + /// [Open online documentation to see images] + /// + [JsonMember] + public float maxEdgeLength = 20; + + /// + /// Minumum region size. + /// Small regions will be removed from the navmesh. + /// Measured in voxels. + /// + /// [Open online documentation to see images] + /// + /// If a region is adjacent to a tile border, it will not be removed + /// even though it is small since the adjacent tile might join it + /// to form a larger region. + /// + /// [Open online documentation to see images] + /// [Open online documentation to see images] + /// + [JsonMember] + public float minRegionSize = 3; + + /// + /// Size in voxels of a single tile. + /// This is the width of the tile. + /// + /// [Open online documentation to see images] + /// + /// A large tile size can be faster to initially scan (but beware of out of memory issues if you try with a too large tile size in a large world) + /// smaller tile sizes are (much) faster to update. + /// + /// Different tile sizes can affect the quality of paths. It is often good to split up huge open areas into several tiles for + /// better quality paths, but too small tiles can also lead to effects looking like invisible obstacles. + /// For more information about this take a look at navmeshnotes (view in online documentation for working links). + /// Usually it is best to experiment and see what works best for your game. + /// + /// When scanning a recast graphs individual tiles can be calculated in parallel which can make it much faster to scan large worlds. + /// When you want to recalculate a part of a recast graph, this can only be done on a tile-by-tile basis which means that if you often try to update a region + /// of the recast graph much smaller than the tile size, then you will be doing a lot of unnecessary calculations. However if you on the other hand + /// update regions of the recast graph that are much larger than the tile size then it may be slower than necessary as there is some overhead in having lots of tiles + /// instead of a few larger ones (not that much though). + /// + /// Recommended values are between 64 and 256, but these are very soft limits. It is possible to use both larger and smaller values. + /// + [JsonMember] + public int editorTileSize = 128; + + /// + /// Size of a tile along the X axis in voxels. + /// \copydetails editorTileSize + /// + /// Warning: Do not modify, it is set from at Scan + /// + /// See: + /// + [JsonMember] + public int tileSizeX = 128; + + /// + /// Size of a tile along the Z axis in voxels. + /// \copydetails editorTileSize + /// + /// Warning: Do not modify, it is set from at Scan + /// + /// See: + /// + [JsonMember] + public int tileSizeZ = 128; + + + /// + /// If true, divide the graph into tiles, otherwise use a single tile covering the whole graph. + /// + /// Using tiles is useful for a number of things. But it also has some drawbacks. + /// - Using tiles allows you to update only a part of the graph at a time. When doing graph updates on a recast graph, it will always recalculate whole tiles (or the whole graph if there are no tiles). + /// components also work on a tile-by-tile basis. + /// - Using tiles allows you to use s. + /// - Using tiles can break up very large triangles, which can improve path quality in some cases, and make the navmesh more closely follow the y-coordinates of the ground. + /// - Using tiles can make it much faster to generate the navmesh, because each tile can be calculated in parallel. + /// But if the tiles are made too small, then the overhead of having many tiles can make it slower than having fewer tiles. + /// - Using small tiles can make the path quality worse in some cases, but setting the s quality setting to high (or using ) will mostly mitigate this. + /// + /// See: + /// + /// Since: Since 4.1 the default value is true. + /// + [JsonMember] + public bool useTiles = true; + + /// + /// If true, scanning the graph will yield a completely empty graph. + /// Useful if you want to replace the graph with a custom navmesh for example + /// + /// Note: This is mostly obsolete now that the and functions exist. + /// + public bool scanEmptyGraph; + + public enum RelevantGraphSurfaceMode { + /// No RelevantGraphSurface components are required anywhere + DoNotRequire, + /// + /// Any surfaces that are completely inside tiles need to have a component + /// positioned on that surface, otherwise it will be stripped away. + /// + OnlyForCompletelyInsideTile, + /// + /// All surfaces need to have one component + /// positioned somewhere on the surface and in each tile that it touches, otherwise it will be stripped away. + /// Only tiles that have a RelevantGraphSurface component for that surface will keep it. + /// + RequireForAll + } + + /// Whether to use 3D or 2D mode + public enum DimensionMode { + /// Allows the recast graph to use 2D colliders + Dimension2D, + /// Allows the recast graph to use 3D colliders, 3D meshes and terrains + Dimension3D, + } + + /// + /// Whether the base of the graph should default to being walkable or unwalkable. + /// + /// See: + /// + public enum BackgroundTraversability { + /// Makes the background walkable by default + Walkable, + /// Makes the background unwalkable by default + Unwalkable, + } + + /// + /// Per layer modification settings. + /// + /// This can be used to make all surfaces with a specific layer get a specific pathfinding tag for example. + /// Or make all surfaces with a specific layer unwalkable. + /// + /// See: If you instead want to apply similar settings on an object level, you can use the component. + /// + [System.Serializable] + public struct PerLayerModification { + /// Layer that this modification applies to + public int layer; + /// \copydocref{RecastMeshObj.mode} + public RecastMeshObj.Mode mode; + /// \copydocref{RecastMeshObj.surfaceID} + public int surfaceID; + + public static PerLayerModification Default => new PerLayerModification { + layer = 0, + mode = RecastMeshObj.Mode.WalkableSurface, + surfaceID = 1, + }; + + public static PerLayerModification[] ToLayerLookup (List perLayerModifications, PerLayerModification defaultValue) { + var lookup = new PerLayerModification[32]; + int seen = 0; + for (int i = 0; i < lookup.Length; i++) { + lookup[i] = defaultValue; + lookup[i].layer = i; + } + for (int i = 0; i < perLayerModifications.Count; i++) { + if (perLayerModifications[i].layer < 0 || perLayerModifications[i].layer >= 32) { + Debug.LogError("Layer " + perLayerModifications[i].layer + " is out of range. Layers must be in the range [0...31]"); + continue; + } + if ((seen & (1 << perLayerModifications[i].layer)) != 0) { + Debug.LogError("Several per layer modifications refer to the same layer '" + LayerMask.LayerToName(perLayerModifications[i].layer) + "'"); + continue; + } + seen |= 1 << perLayerModifications[i].layer; + lookup[perLayerModifications[i].layer] = perLayerModifications[i]; + } + return lookup; + } + } + + /// Settings for which meshes/colliders and other objects to include in the graph + [System.Serializable] + public class CollectionSettings { + /// Determines how the initial filtering of objects is done + public enum FilterMode { + /// Use a layer mask to filter objects + Layers, + /// Use tags to filter objects + Tags, + } + + /// + /// Determines how the initial filtering of objects is done. + /// + /// See: + /// See: + /// + public FilterMode collectionMode = FilterMode.Layers; + + /// + /// Objects in all of these layers will be rasterized. + /// + /// Will only be used if is set to Layers. + /// + /// See: + /// + public LayerMask layerMask = -1; + + /// + /// Objects tagged with any of these tags will be rasterized. + /// + /// Will only be used if is set to Tags. + /// + /// See: + /// + public List tagMask = new List(); + + /// + /// Use colliders to calculate the navmesh. + /// + /// Depending on the , either 3D or 2D colliders will be rasterized. + /// + /// Sphere/Capsule/Circle colliders will be approximated using polygons, with the precision specified in . + /// + /// Note: In 2D mode, this is always treated as enabled, because no other types of inputs (like meshes or terrains) are supported. + /// + public bool rasterizeColliders; + + /// + /// Use scene meshes to calculate the navmesh. + /// + /// This can get you higher precision than colliders, since colliders are typically very simplified versions of the mesh. + /// However, it is often slower to scan, and graph updates can be particularly slow. + /// + /// The reason that graph updates are slower is that there's no efficient way to find all meshes that intersect a given tile, + /// so the graph has to iterate over all meshes in the scene just to find the ones relevant for the tiles that you want to update. + /// Colliders, on the other hand, can be efficiently queried using the physics system. + /// + /// You can disable this and attach a component (with dynamic=false) to all meshes that you want to be included in the navmesh instead. + /// That way they will be able to be efficiently queried for, without having to iterate through all meshes in the scene. + /// + /// In 2D mode, this setting has no effect. + /// + public bool rasterizeMeshes = true; + + /// + /// Use terrains to calculate the navmesh. + /// + /// In 2D mode, this setting has no effect. + /// + public bool rasterizeTerrain = true; + + /// + /// Rasterize tree colliders on terrains. + /// + /// If the tree prefab has a collider, that collider will be rasterized. + /// Otherwise a simple box collider will be used and the script will + /// try to adjust it to the tree's scale, it might not do a very good job though so + /// an attached collider is preferable. + /// + /// Note: It seems that Unity will only generate tree colliders at runtime when the game is started. + /// For this reason, this graph will not pick up tree colliders when scanned outside of play mode + /// but it will pick them up if the graph is scanned when the game has started. If it still does not pick them up + /// make sure that the trees actually have colliders attached to them and that the tree prefabs are + /// in the correct layer (the layer should be included in the layer mask). + /// + /// In 2D mode, this setting has no effect. + /// + /// See: + /// See: + /// + public bool rasterizeTrees = true; + + /// + /// Controls how much to downsample the terrain's heightmap before generating the input mesh used for rasterization. + /// A higher value is faster to scan but less accurate. + /// + public int terrainHeightmapDownsamplingFactor = 3; + + /// + /// Controls detail on rasterization of sphere and capsule colliders. + /// + /// The colliders will be approximated with polygons so that the max distance to the theoretical surface is less than 1/(this number of voxels). + /// + /// A higher value does not necessarily increase quality of the mesh, but a lower + /// value will often speed it up. + /// + /// You should try to keep this value as low as possible without affecting the mesh quality since + /// that will yield the fastest scan times. + /// + /// The default value is 1, which corresponds to a maximum error of 1 voxel. + /// In most cases, increasing this to a value higher than 2 (corresponding to a maximum error of 0.5 voxels) is not useful. + /// + /// See: rasterizeColliders + /// + /// Version: Before 4.3.80 this variable was not scaled by the , and so it would not transfer as easily between scenes of different scales. + /// + public float colliderRasterizeDetail = 1; + + /// + /// Callback for collecting custom scene meshes. + /// + /// This callback will be called once when scanning the graph, to allow you to add custom meshes to the graph, and once every time a graph update happens. + /// Use the class to add meshes that are to be rasterized. + /// + /// Note: This is a callback, and can therefore not be serialized. You must set this field using code, every time the game starts (and optionally in edit mode as well). + /// + /// + /// AstarPath.active.data.recastGraph.collectionSettings.onCollectMeshes += (RecastMeshGatherer gatherer) => { + /// // Define a mesh using 4 vertices and 2 triangles + /// var vertices = new Vector3[] { + /// new Vector3(0, 0, 0), + /// new Vector3(100, 0, 0), + /// new Vector3(100, 0, 100), + /// new Vector3(0, 0, 100) + /// }; + /// var triangles = new int[] { 0, 1, 2, 0, 2, 3 }; + /// // Register the mesh buffers + /// var meshDataIndex = gatherer.AddMeshBuffers(vertices, triangles); + /// // Register the mesh for rasterization + /// gatherer.AddMesh(new RecastMeshGatherer.GatheredMesh { + /// meshDataIndex = meshDataIndex, + /// area = 0, + /// indexStart = 0, + /// indexEnd = -1, + /// bounds = default, + /// matrix = Matrix4x4.identity, + /// solid = false, + /// doubleSided = true, + /// flatten = false, + /// areaIsTag = false + /// }); + /// }; + /// AstarPath.active.Scan(); + /// + /// + public System.Action onCollectMeshes; + } + + /// + /// List of rules that modify the graph based on the layer of the rasterized object. + /// + /// [Open online documentation to see images] + /// + /// By default, all layers are treated as walkable surfaces. + /// But by adding rules to this list, one can for example make all surfaces with a specific layer get a specific pathfinding tag. + /// + /// Each layer should be modified at most once in this list. + /// + /// If an object has a component attached, the settings on that component will override the settings in this list. + /// + /// See: + /// + [JsonMember] + public List perLayerModifications = new List(); + + /// + /// Whether to use 3D or 2D mode. + /// + /// See: + /// + [JsonMember] + public DimensionMode dimensionMode = DimensionMode.Dimension3D; + + /// + /// Whether the base of the graph should default to being walkable or unwalkable. + /// + /// This is only used in 2D mode. In 3D mode, this setting has no effect. + /// + /// For 2D games, it can be very useful to set the background to be walkable by default, and then + /// constrain walkability using colliders. + /// + /// If you don't want to use a walkable background, you can instead create colliders and attach a RecastMeshObj with Surface Type set to Walkable Surface. + /// These will then create walkable regions. + /// + /// See: + /// + [JsonMember] + public BackgroundTraversability backgroundTraversability = BackgroundTraversability.Walkable; + + /// + /// Require every region to have a RelevantGraphSurface component inside it. + /// A RelevantGraphSurface component placed in the scene specifies that + /// the navmesh region it is inside should be included in the navmesh. + /// + /// If this is set to OnlyForCompletelyInsideTile + /// a navmesh region is included in the navmesh if it + /// has a RelevantGraphSurface inside it, or if it + /// is adjacent to a tile border. This can leave some small regions + /// which you didn't want to have included because they are adjacent + /// to tile borders, but it removes the need to place a component + /// in every single tile, which can be tedious (see below). + /// + /// If this is set to RequireForAll + /// a navmesh region is included only if it has a RelevantGraphSurface + /// inside it. Note that even though the navmesh + /// looks continous between tiles, the tiles are computed individually + /// and therefore you need a RelevantGraphSurface component for each + /// region and for each tile. + /// + /// [Open online documentation to see images] + /// In the above image, the mode OnlyForCompletelyInsideTile was used. Tile borders + /// are highlighted in black. Note that since all regions are adjacent to a tile border, + /// this mode didn't remove anything in this case and would give the same result as DoNotRequire. + /// The RelevantGraphSurface component is shown using the green gizmo in the top-right of the blue plane. + /// + /// [Open online documentation to see images] + /// In the above image, the mode RequireForAll was used. No tiles were used. + /// Note that the small region at the top of the orange cube is now gone, since it was not the in the same + /// region as the relevant graph surface component. + /// The result would have been identical with OnlyForCompletelyInsideTile since there are no tiles (or a single tile, depending on how you look at it). + /// + /// [Open online documentation to see images] + /// The mode RequireForAll was used here. Since there is only a single RelevantGraphSurface component, only the region + /// it was in, in the tile it is placed in, will be enabled. If there would have been several RelevantGraphSurface in other tiles, + /// those regions could have been enabled as well. + /// + /// [Open online documentation to see images] + /// Here another tile size was used along with the OnlyForCompletelyInsideTile. + /// Note that the region on top of the orange cube is gone now since the region borders do not intersect that region (and there is no + /// RelevantGraphSurface component inside it). + /// + /// Note: When not using tiles. OnlyForCompletelyInsideTile is equivalent to RequireForAll. + /// + [JsonMember] + public RelevantGraphSurfaceMode relevantGraphSurfaceMode = RelevantGraphSurfaceMode.DoNotRequire; + + /// + /// Determines which objects are used to build the graph, when it is scanned. + /// + /// Also contains some settings for how to convert objects into meshes. + /// Spherical colliders, for example, need to be converted into a triangular mesh before they can be used in the graph. + /// + /// See: + /// + [JsonMember] + public CollectionSettings collectionSettings = new CollectionSettings(); + + /// + /// Use colliders to calculate the navmesh. + /// + /// Depending on the , either 3D or 2D colliders will be rasterized. + /// + /// Sphere/Capsule/Circle colliders will be approximated using polygons, with the precision specified in . + /// Deprecated: Use instead + /// + [System.Obsolete("Use collectionSettings.rasterizeColliders instead")] + public bool rasterizeColliders { + get => collectionSettings.rasterizeColliders; + set => collectionSettings.rasterizeColliders = value; + } + + /// + /// Use scene meshes to calculate the navmesh. + /// + /// This can get you higher precision than colliders, since colliders are typically very simplified versions of the mesh. + /// However, it is often slower to scan, and graph updates can be particularly slow. + /// + /// The reason that graph updates are slower is that there's no efficient way to find all meshes that intersect a given tile, + /// so the graph has to iterate over all meshes in the scene just to find the ones relevant for the tiles that you want to update. + /// Colliders, on the other hand, can be efficiently queried using the physics system. + /// + /// You can disable this and attach a component (with dynamic=false) to all meshes that you want to be included in the navmesh instead. + /// That way they will be able to be efficiently queried for, without having to iterate through all meshes in the scene. + /// + /// In 2D mode, this setting has no effect. + /// Deprecated: Use instead + /// + [System.Obsolete("Use collectionSettings.rasterizeMeshes instead")] + public bool rasterizeMeshes { + get => collectionSettings.rasterizeMeshes; + set => collectionSettings.rasterizeMeshes = value; + } + + /// + /// Use terrains to calculate the navmesh. + /// + /// In 2D mode, this setting has no effect. + /// Deprecated: Use instead + /// + [System.Obsolete("Use collectionSettings.rasterizeTerrain instead")] + public bool rasterizeTerrain { + get => collectionSettings.rasterizeTerrain; + set => collectionSettings.rasterizeTerrain = value; + } + + /// + /// Rasterize tree colliders on terrains. + /// + /// If the tree prefab has a collider, that collider will be rasterized. + /// Otherwise a simple box collider will be used and the script will + /// try to adjust it to the tree's scale, it might not do a very good job though so + /// an attached collider is preferable. + /// + /// Note: It seems that Unity will only generate tree colliders at runtime when the game is started. + /// For this reason, this graph will not pick up tree colliders when scanned outside of play mode + /// but it will pick them up if the graph is scanned when the game has started. If it still does not pick them up + /// make sure that the trees actually have colliders attached to them and that the tree prefabs are + /// in the correct layer (the layer should be included in the layer mask). + /// + /// In 2D mode, this setting has no effect. + /// + /// See: + /// See: + /// Deprecated: Use instead + /// + [System.Obsolete("Use collectionSettings.rasterizeTrees instead")] + public bool rasterizeTrees { + get => collectionSettings.rasterizeTrees; + set => collectionSettings.rasterizeTrees = value; + } + + /// + /// Controls detail on rasterization of sphere and capsule colliders. + /// + /// The colliders will be approximated with polygons so that the max distance to the theoretical surface is less than 1/(this number of voxels). + /// + /// A higher value does not necessarily increase quality of the mesh, but a lower + /// value will often speed it up. + /// + /// You should try to keep this value as low as possible without affecting the mesh quality since + /// that will yield the fastest scan times. + /// + /// The default value is 1, which corresponds to a maximum error of 1 voxel. + /// In most cases, increasing this to a value higher than 2 (corresponding to a maximum error of 0.5 voxels) is not useful. + /// + /// See: rasterizeColliders + /// + /// Version: Before 4.3.80 this variable was not scaled by the , and so it would not transfer as easily between scenes of different scales. + /// + /// Deprecated: Use instead + /// + [System.Obsolete("Use collectionSettings.colliderRasterizeDetail instead")] + public float colliderRasterizeDetail { + get => collectionSettings.colliderRasterizeDetail; + set => collectionSettings.colliderRasterizeDetail = value; + } + + /// + /// Layer mask which filters which objects to include. + /// See: + /// Deprecated: Use instead + /// + [System.Obsolete("Use collectionSettings.layerMask instead")] + public LayerMask mask { + get => collectionSettings.layerMask; + set => collectionSettings.layerMask = value; + } + + /// + /// Objects tagged with any of these tags will be rasterized. + /// Note that this extends the layer mask, so if you only want to use tags, set to 'Nothing'. + /// + /// See: + /// Deprecated: Use instead + /// + [System.Obsolete("Use collectionSettings.tagMask instead")] + public List tagMask { + get => collectionSettings.tagMask; + set => collectionSettings.tagMask = value; + } + + /// + /// Controls how large the sample size for the terrain is. + /// A higher value is faster to scan but less accurate. + /// + /// The heightmap resolution is effectively divided by this value, before the terrain is rasterized. + /// + /// Deprecated: Use instead + /// + [System.Obsolete("Use collectionSettings.terrainHeightmapDownsamplingFactor instead")] + public int terrainSampleSize { + get => collectionSettings.terrainHeightmapDownsamplingFactor; + set => collectionSettings.terrainHeightmapDownsamplingFactor = value; + } + + /// Rotation of the graph in degrees + [JsonMember] + public Vector3 rotation; + + /// + /// Center of the bounding box. + /// Scanning will only be done inside the bounding box + /// + [JsonMember] + public Vector3 forcedBoundsCenter; + +#if UNITY_EDITOR + /// Internal field used to warn users when the mesh includes meshes that are not readable at runtime + public List<(UnityEngine.Object, Mesh)> meshesUnreadableAtRuntime; +#endif + + public override float NavmeshCuttingCharacterRadius => characterRadius; + + public override bool RecalculateNormals { get { return true; } } + + public override float TileWorldSizeX { + get { + return tileSizeX*cellSize; + } + } + + public override float TileWorldSizeZ { + get { + return tileSizeZ*cellSize; + } + } + + public override float MaxTileConnectionEdgeDistance { + get { + return walkableClimb; + } + } + + /// + /// World bounding box for the graph. + /// + /// This always contains the whole graph. + /// + /// Note: Since this is an axis-aligned bounding box, it may not be particularly tight if the graph is significantly rotated. + /// + /// [Open online documentation to see images] + /// + public override Bounds bounds { + get { + var m = (float4x4)CalculateTransform().matrix; + var b = new ToWorldMatrix(new float3x3(m.c0.xyz, m.c1.xyz, m.c2.xyz)).ToWorld(new Bounds(Vector3.zero, forcedBoundsSize)); + b.center += forcedBoundsCenter; + return b; + } + } + + /// + /// True if the point is inside the bounding box of this graph. + /// + /// Note: This method uses a tighter non-axis-aligned bounding box than you can get from the property. + /// + /// Note: What is considered inside the bounds is only updated when the graph is scanned. For an unscanned graph, this will always return false. + /// + /// In 2D mode, the point is considered inside if it is contained along the graph's X and Z axes (Y is ignored). + /// Note that the graph's X and Z axes are typically aligned with the world's X and Y axes when using 2D mode. + /// + public override bool IsInsideBounds (Vector3 point) { + if (this.tiles == null || this.tiles.Length == 0) return false; + + var local = (float3)transform.InverseTransform(point); + if (dimensionMode == DimensionMode.Dimension2D) { + return local.x >= 0 && local.z >= 0 && local.x <= forcedBoundsSize.x && local.z <= forcedBoundsSize.z; + } else { + return math.all(local >= 0) && math.all(local <= (float3)forcedBoundsSize); + } + } + + /// + /// Changes the bounds of the graph to precisely encapsulate all objects in the scene that can be included in the scanning process based on the settings. + /// Which objects are used depends on the settings. If an object would have affected the graph with the current settings if it would have + /// been inside the bounds of the graph, it will be detected and the bounds will be expanded to contain that object. + /// + /// This method corresponds to the 'Snap bounds to scene' button in the inspector. + /// + /// See: rasterizeMeshes + /// See: rasterizeTerrain + /// See: rasterizeColliders + /// See: mask + /// See: tagMask + /// + /// See: forcedBoundsCenter + /// See: forcedBoundsSize + /// + public void SnapForceBoundsToScene () { + var arena = new DisposeArena(); + var meshes = new TileBuilder(this, new TileLayout(this), default).CollectMeshes(new Bounds(Vector3.zero, new Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity))); + + if (meshes.meshes.Length > 0) { + // Project all bounding boxes into a space relative to the current rotation of the graph + var m = new ToWorldMatrix(new float3x3((quaternion)Quaternion.Inverse(Quaternion.Euler(rotation)))); + var bounds = m.ToWorld(meshes.meshes[0].bounds); + + for (int i = 1; i < meshes.meshes.Length; i++) { + bounds.Encapsulate(m.ToWorld(meshes.meshes[i].bounds)); + } + + // The center is in world space, so we need to convert it back from the rotated space + forcedBoundsCenter = Quaternion.Euler(rotation) * bounds.center; + forcedBoundsSize = bounds.size; + } + + arena.Add(meshes); + arena.DisposeAll(); + } + + DisposeArena pendingGraphUpdateArena = new DisposeArena(); + + class RecastGraphUpdatePromise : IGraphUpdatePromise { + public List<(Promise, Promise)> promises; + public List graphUpdates; + public RecastGraph graph; + int graphHash; + + public RecastGraphUpdatePromise (RecastGraph graph, List graphUpdates) { + this.promises = ListPool<(Promise, Promise)>.Claim(); + this.graph = graph; + this.graphHash = HashSettings(graph); + var tileRecalculations = ListPool<(IntRect, GraphUpdateObject)>.Claim(); + for (int i = graphUpdates.Count - 1; i >= 0; i--) { + var guo = graphUpdates[i]; + if (guo.updatePhysics) { + graphUpdates.RemoveAt(i); + + // Calculate world bounds of all affected tiles + // Expand TileBorderSizeInWorldUnits voxels in all directions to make sure + // all tiles that could be affected by the update are recalculated. + // TODO: Shouldn't this be expanded by the character radius too? + IntRect touchingTiles = graph.GetTouchingTiles(guo.bounds, graph.TileBorderSizeInWorldUnits); + if (touchingTiles.IsValid()) { + tileRecalculations.Add((touchingTiles, guo)); + } + } + } + + this.graphUpdates = graphUpdates; + // Sort larger updates first + if (tileRecalculations.Count > 1) tileRecalculations.Sort((a, b) => b.Item1.Area.CompareTo(a.Item1.Area)); + + for (int i = 0; i < tileRecalculations.Count; i++) { + var(touchingTiles, guo) = tileRecalculations[i]; + + // Skip this graph update if we have already scheduled an update that + // covers the same tiles + if (tileRecalculations.Count > 1) { + bool anyNew = false; + for (int z = touchingTiles.ymin; z <= touchingTiles.ymax; z++) { + for (int x = touchingTiles.xmin; x <= touchingTiles.xmax; x++) { + var tile = graph.GetTile(x, z); + anyNew |= !tile.flag; + tile.flag = true; + } + } + + if (!anyNew) continue; + } + + var tileLayout = new TileLayout(graph); + var pendingGraphUpdatePromise = RecastBuilder.BuildTileMeshes(graph, tileLayout, touchingTiles).Schedule(graph.pendingGraphUpdateArena); + var pendingGraphUpdatePromise2 = RecastBuilder.BuildNodeTiles(graph, tileLayout).Schedule(graph.pendingGraphUpdateArena, pendingGraphUpdatePromise); + promises.Add((pendingGraphUpdatePromise, pendingGraphUpdatePromise2)); + } + + if (tileRecalculations.Count > 1) { + for (int i = 0; i < tileRecalculations.Count; i++) { + var(touchingTiles, _) = tileRecalculations[i]; + + for (int z = touchingTiles.ymin; z <= touchingTiles.ymax; z++) { + for (int x = touchingTiles.xmin; x <= touchingTiles.xmax; x++) { + graph.GetTile(x, z).flag = false; + } + } + } + } + + ListPool<(IntRect, GraphUpdateObject)>.Release(ref tileRecalculations); + } + + public IEnumerator Prepare () { + for (int i = 0; i < promises.Count; i++) { + yield return promises[i].Item2.handle; + yield return promises[i].Item1.handle; + } + } + + static int HashSettings(RecastGraph graph) => (((graph.tileXCount * 31) ^ graph.tileZCount) * 31 ^ graph.TileWorldSizeX.GetHashCode() * 31) ^ graph.TileWorldSizeZ.GetHashCode(); + + public void Apply (IGraphUpdateContext ctx) { + if (HashSettings(graph) != graphHash) throw new System.InvalidOperationException("Recast graph changed while a graph update was in progress. This is not allowed. Use AstarPath.active.AddWorkItem if you need to update graphs."); + + for (int i = 0; i < promises.Count; i++) { + var promise1 = promises[i].Item1; + var promise2 = promises[i].Item2; + Profiler.BeginSample("Applying graph update results"); + var tilesResult = promise2.Complete(); + var tileRect = tilesResult.dependency.tileMeshes.tileRect; + + var tiles = tilesResult.tiles; + promise1.Dispose(); + promise2.Dispose(); + + // Initialize all nodes that were created in the jobs + for (int j = 0; j < tiles.Length; j++) AstarPath.active.InitializeNodes(tiles[j].nodes); + + // Assign all tiles to the graph. + // Remove connections from existing tiles destroy the nodes + // Replace the old tile by the new tile + graph.StartBatchTileUpdate(); + for (int z = 0; z < tileRect.Height; z++) { + for (int x = 0; x < tileRect.Width; x++) { + var tileIndex = (z+tileRect.ymin)*graph.tileXCount + (x + tileRect.xmin); + graph.ClearTile(x + tileRect.xmin, z+tileRect.ymin); + + var newTile = tiles[z*tileRect.Width + x]; + // Assign the new tile + newTile.graph = graph; + graph.tiles[tileIndex] = newTile; + } + } + graph.EndBatchTileUpdate(); + + + // All tiles inside the update will already be connected to each other + // but they will not be connected to any tiles outside the update. + // We do this here. It needs to be done as one atomic update on the Unity main thread + // because other code may be reading graph data on the main thread. + var tilesHandle = System.Runtime.InteropServices.GCHandle.Alloc(graph.tiles); + var graphTileRect = new IntRect(0, 0, graph.tileXCount - 1, graph.tileZCount - 1); + JobConnectTiles.ScheduleRecalculateBorders(tilesHandle, default, graphTileRect, tileRect, new Vector2(graph.TileWorldSizeX, graph.TileWorldSizeZ), graph.MaxTileConnectionEdgeDistance).Complete(); + tilesHandle.Free(); + + // Signal that tiles have been recalculated to the navmesh cutting system. + // This may be used to update the tile again to take into + // account NavmeshCut components. + // It is not the super efficient, but it works. + // Usually you only use either normal graph updates OR navmesh + // cutting, not both. + graph.navmeshUpdateData.OnRecalculatedTiles(tiles); + if (graph.OnRecalculatedTiles != null) graph.OnRecalculatedTiles(tiles); + ctx.DirtyBounds(graph.GetTileBounds(tileRect)); + + Profiler.EndSample(); + } + + graph.pendingGraphUpdateArena.DisposeAll(); + + if (graphUpdates != null) { + for (int i = 0; i < graphUpdates.Count; i++) { + var guo = graphUpdates[i]; + + // Figure out which tiles are affected + // Expand TileBorderSizeInWorldUnits voxels in all directions to make sure + // all tiles that could be affected by the update are recalculated. + var affectedTiles = graph.GetTouchingTiles(guo.bounds, graph.TileBorderSizeInWorldUnits); + + // If the bounding box did not overlap with the graph then just skip the update + if (!affectedTiles.IsValid()) continue; + + for (int z = affectedTiles.ymin; z <= affectedTiles.ymax; z++) { + for (int x = affectedTiles.xmin; x <= affectedTiles.xmax; x++) { + NavmeshTile tile = graph.tiles[z*graph.tileXCount + x]; + NavMeshGraph.UpdateArea(guo, tile); + } + } + ctx.DirtyBounds(graph.GetTileBounds(affectedTiles)); + } + } + } + } + + IGraphUpdatePromise IUpdatableGraph.ScheduleGraphUpdates(List graphUpdates) => new RecastGraphUpdatePromise(this, graphUpdates); + + class RecastGraphScanPromise : IGraphUpdatePromise { + public RecastGraph graph; + TileLayout tileLayout; + bool emptyGraph; + NavmeshTile[] tiles; + IProgress progressSource; + + public float Progress => progressSource != null ? progressSource.Progress : 1; + +#if UNITY_EDITOR + List<(UnityEngine.Object, Mesh)> meshesUnreadableAtRuntime; +#endif + + public IEnumerator Prepare () { + TriangleMeshNode.SetNavmeshHolder(AstarPath.active.data.GetGraphIndex(graph), graph); + + if (!Application.isPlaying) { + RelevantGraphSurface.FindAllGraphSurfaces(); + } + + RelevantGraphSurface.UpdateAllPositions(); + + tileLayout = new TileLayout(graph); + + // If this is true, just fill the graph with empty tiles + if (graph.scanEmptyGraph || tileLayout.tileCount.x*tileLayout.tileCount.y <= 0) { + emptyGraph = true; + yield break; + } + + var arena = new DisposeArena(); + var tileMeshesPromise = RecastBuilder.BuildTileMeshes(graph, tileLayout, new IntRect(0, 0, tileLayout.tileCount.x - 1, tileLayout.tileCount.y - 1)).Schedule(arena); + var buildNodesJob = RecastBuilder.BuildNodeTiles(graph, tileLayout); + var tilesPromise = buildNodesJob.Schedule(arena, tileMeshesPromise); + progressSource = tilesPromise; + yield return tilesPromise.handle; + + progressSource = null; + var tiles = tilesPromise.Complete(); + var tileMeshes = tileMeshesPromise.Complete(); + this.tiles = tiles.tiles; + +#if UNITY_EDITOR + meshesUnreadableAtRuntime = tileMeshes.meshesUnreadableAtRuntime; + tileMeshes.meshesUnreadableAtRuntime = null; +#endif + + tileMeshes.Dispose(); + tiles.Dispose(); + arena.DisposeAll(); + } + + public void Apply (IGraphUpdateContext ctx) { + // Destroy all previous nodes, if any exist + graph.DestroyAllNodes(); + + if (emptyGraph) { + graph.SetLayout(tileLayout); + graph.FillWithEmptyTiles(); + } else { + // Initialize all nodes that were created in the jobs + for (int j = 0; j < tiles.Length; j++) AstarPath.active.InitializeNodes(tiles[j].nodes); + +#if UNITY_EDITOR + graph.meshesUnreadableAtRuntime = meshesUnreadableAtRuntime; +#endif + + // Assign all tiles to the graph + // We do this in a single atomic update (from the main thread's perspective) to ensure + // that even if one does an async scan, the graph will always be in a valid state. + // This guarantees that things like GetNearest will still work during an async scan. + graph.SetLayout(tileLayout); + graph.tiles = tiles; + for (int i = 0; i < tiles.Length; i++) tiles[i].graph = graph; + } + + // Signal that tiles have been recalculated to the navmesh cutting system. + graph.navmeshUpdateData.OnRecalculatedTiles(graph.tiles); + if (graph.OnRecalculatedTiles != null) graph.OnRecalculatedTiles(graph.tiles.Clone() as NavmeshTile[]); + } + } + + /// + /// Moves the recast graph by a number of tiles, discarding old tiles and scanning new ones. + /// + /// Note: Only translation in a single direction is supported. dx == 0 || dz == 0 must hold. + /// If you need to move the graph diagonally, then you can call this function twice, once for each axis. + /// + /// This is used by the component to efficiently move the graph. + /// + /// All tiles that can stay in the same position will stay. The ones that would have fallen off the edge of the graph will be discarded, + /// and new tiles will be created and scanned at the other side of the graph. + /// + /// See: + /// + /// Returns: An async graph update promise. See + /// + /// Number of tiles along the graph's X axis to move by. + /// Number of tiles along the graph's Z axis to move by. + public IGraphUpdatePromise TranslateInDirection (int dx, int dz) { + return new RecastMovePromise(this, new Int2(dx, dz)); + } + + class RecastMovePromise : IGraphUpdatePromise { + RecastGraph graph; + TileMeshes tileMeshes; + Int2 delta; + IntRect newTileRect; + + public RecastMovePromise(RecastGraph graph, Int2 delta) { + this.graph = graph; + this.delta = delta; + if (delta.x != 0 && delta.y != 0) throw new System.ArgumentException("Only translation in a single direction is supported. delta.x == 0 || delta.y == 0 must hold."); + } + + public IEnumerator Prepare () { + if (delta.x == 0 && delta.y == 0) yield break; + + var originalTileRect = new IntRect(0, 0, graph.tileXCount - 1, graph.tileZCount - 1); + newTileRect = originalTileRect.Offset(delta); + var createdTiles = IntRect.Exclude(newTileRect, originalTileRect); + + var disposeArena = new DisposeArena(); + + var buildSettings = RecastBuilder.BuildTileMeshes(graph, new TileLayout(graph), createdTiles); + buildSettings.scene = graph.active.gameObject.scene; + + // Schedule the jobs asynchronously. + // These jobs will prepare the data for the update, but will not change any graph data. + // This is to ensure that the graph data stays valid even if the update takes multiple frames. + // Any changes will be made in the #Apply method. + var pendingPromise = buildSettings.Schedule(disposeArena); + + // Wait for the job to complete + yield return pendingPromise.handle; + + var output = pendingPromise.GetValue(); + tileMeshes = output.tileMeshes.ToManaged(); + pendingPromise.Dispose(); + disposeArena.DisposeAll(); + // Set the tile rect of the newly created tiles relative to the #newTileRect + tileMeshes.tileRect = createdTiles.Offset(originalTileRect.Min - newTileRect.Min); + } + + public void Apply (IGraphUpdateContext ctx) { + if (delta.x == 0 && delta.y == 0) return; + + graph.Resize(newTileRect); + graph.ReplaceTiles(tileMeshes); + } + } + + protected override IGraphUpdatePromise ScanInternal (bool async) => new RecastGraphScanPromise { graph = this }; + + public override GraphTransform CalculateTransform () { + return CalculateTransform(new Bounds(forcedBoundsCenter, forcedBoundsSize), Quaternion.Euler(rotation)); + } + + public static GraphTransform CalculateTransform (Bounds bounds, Quaternion rotation) { + return new GraphTransform(Matrix4x4.TRS(bounds.center, rotation, Vector3.one) * Matrix4x4.TRS(-bounds.extents, Quaternion.identity, Vector3.one)); + } + + protected void SetLayout (TileLayout info) { + this.tileXCount = info.tileCount.x; + this.tileZCount = info.tileCount.y; + this.tileSizeX = info.tileSizeInVoxels.x; + this.tileSizeZ = info.tileSizeInVoxels.y; + this.transform = info.transform; + } + + /// Convert character radius to a number of voxels + internal int CharacterRadiusInVoxels { + get { + // Round it up most of the time, but round it down + // if it is very close to the result when rounded down + return Mathf.CeilToInt((characterRadius / cellSize) - 0.1f); + } + } + + /// + /// 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) + /// + internal int TileBorderSizeInVoxels { + get { + return CharacterRadiusInVoxels + 3; + } + } + + internal float TileBorderSizeInWorldUnits { + get { + return TileBorderSizeInVoxels*cellSize; + } + } + + /// + /// Resize the number of tiles that this graph contains. + /// + /// This can be used both to make a graph larger, smaller or move the bounds of the graph around. + /// The new bounds are relative to the existing bounds which are IntRect(0, 0, tileCountX-1, tileCountZ-1). + /// + /// Any current tiles that fall outside the new bounds will be removed. + /// Any new tiles that did not exist inside the previous bounds will be created as empty tiles. + /// All other tiles will be preserved. They will stay at their current world space positions. + /// + /// Note: This is intended to be used at runtime on an already scanned graph. + /// If you want to change the bounding box of a graph like in the editor, use and instead. + /// + /// + /// AstarPath.active.AddWorkItem(() => { + /// var graph = AstarPath.active.data.recastGraph; + /// var currentBounds = new IntRect(0, 0, graph.tileXCount-1, graph.tileZCount-1); + /// + /// // Make the graph twice as large, but discard the first 3 columns. + /// // All other tiles will be kept and stay at the same position in the world. + /// // The new tiles will be empty. + /// graph.Resize(new IntRect(3, 0, currentBounds.xmax*2, currentBounds.ymax*2)); + /// }); + /// + /// + /// Rectangle of tiles that the graph should contain. Relative to the old bounds. + public virtual void Resize (IntRect newTileBounds) { + AssertSafeToUpdateGraph(); + + if (!newTileBounds.IsValid()) throw new System.ArgumentException("Invalid tile bounds"); + if (newTileBounds == new IntRect(0, 0, tileXCount-1, tileZCount-1)) return; + if (newTileBounds.Area == 0) throw new System.ArgumentException("Tile count must at least 1x1"); + + StartBatchTileUpdate(); + + // Create a new tile array and copy the old tiles over, and destroy tiles that are outside the new bounds + var newTiles = new NavmeshTile[newTileBounds.Area]; + for (int z = 0; z < tileZCount; z++) { + for (int x = 0; x < tileXCount; x++) { + if (newTileBounds.Contains(x, z)) { + NavmeshTile tile = tiles[x + z*tileXCount]; + newTiles[(x - newTileBounds.xmin) + (z - newTileBounds.ymin)*newTileBounds.Width] = tile; + } else { + ClearTile(x, z); + + // This tile is removed, and that means some off-mesh links may need to be recalculated + DirtyBounds(GetTileBounds(x, z)); + } + } + } + + // Update the graph's bounding box so that it covers the new tiles + this.forcedBoundsSize = new Vector3(newTileBounds.Width*TileWorldSizeX, forcedBoundsSize.y, newTileBounds.Height*TileWorldSizeZ); + this.forcedBoundsCenter = this.transform.Transform( + new Vector3( + (newTileBounds.xmin + newTileBounds.xmax + 1)*0.5f*TileWorldSizeX, + forcedBoundsSize.y*0.5f, + (newTileBounds.ymin + newTileBounds.ymax + 1)*0.5f*TileWorldSizeZ + ) + ); + this.transform = CalculateTransform(); + var offset = -(Int3) new Vector3(TileWorldSizeX * newTileBounds.xmin, 0, TileWorldSizeZ * newTileBounds.ymin); + + // Create new tiles for the new bounds + for (int z = 0; z < newTileBounds.Height; z++) { + for (int x = 0; x < newTileBounds.Width; x++) { + var tileIndex = x + z*newTileBounds.Width; + var tile = newTiles[tileIndex]; + if (tile == null) { + newTiles[tileIndex] = NewEmptyTile(x, z); + } else { + tile.x = x; + tile.z = z; + + // Ensure nodes refer to the correct tile index + for (int i = 0; i < tile.nodes.Length; i++) { + var node = tile.nodes[i]; + // The tile indices change when we resize the graph + node.v0 = (node.v0 & VertexIndexMask) | (tileIndex << TileIndexOffset); + node.v1 = (node.v1 & VertexIndexMask) | (tileIndex << TileIndexOffset); + node.v2 = (node.v2 & VertexIndexMask) | (tileIndex << TileIndexOffset); + } + + // Update the vertex positions in graph space + for (int i = 0; i < tile.vertsInGraphSpace.Length; i++) { + tile.vertsInGraphSpace[i] += offset; + } + + tile.vertsInGraphSpace.CopyTo(tile.verts); + transform.Transform(tile.verts); + + // Recalculate the BBTree, since the vertices have moved in graph space. + // TODO: Should the BBTree be built in tile-space instead, to avoid this recalculation? + tile.bbTree.Dispose(); + tile.bbTree = new BBTree(tile.tris, tile.vertsInGraphSpace); + } + } + } + this.tiles = newTiles; + this.tileXCount = newTileBounds.Width; + this.tileZCount = newTileBounds.Height; + EndBatchTileUpdate(); + this.navmeshUpdateData.OnResized(newTileBounds); + } + + /// Initialize the graph with empty tiles if it is not currently scanned + public void EnsureInitialized () { + AssertSafeToUpdateGraph(); + if (this.tiles == null) { + TriangleMeshNode.SetNavmeshHolder(AstarPath.active.data.GetGraphIndex(this), this); + SetLayout(new TileLayout(this)); + FillWithEmptyTiles(); + } + } + + /// + /// Load tiles from a object into this graph. + /// + /// This can be used for many things, for example world streaming or placing large prefabs that have been pre-scanned. + /// + /// The loaded tiles must have the same world-space size as this graph's tiles. + /// The world-space size for a recast graph is given by the multiplied by (or . + /// + /// If the graph is not scanned when this method is called, the graph will be initialized and consist of just the tiles loaded by this call. + /// + /// + /// // 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: + /// See: + /// See: + /// See: + /// See: + /// See: + /// See: + /// + /// The tiles to load. They will be loaded into the graph at the \reflink{TileMeshes.tileRect} tile coordinates. + /// All vertices in the loaded tiles will be moved upwards (or downwards if negative) by this amount. + public void ReplaceTiles (TileMeshes tileMeshes, float yOffset = 0) { + AssertSafeToUpdateGraph(); + EnsureInitialized(); + + if (tileMeshes.tileWorldSize.x != TileWorldSizeX || tileMeshes.tileWorldSize.y != TileWorldSizeZ) { + throw new System.Exception("Loaded tile size does not match this graph's tile size.\n" + + "The source tiles have a world-space tile size of " + tileMeshes.tileWorldSize + " while this graph's tile size is (" + TileWorldSizeX + "," + TileWorldSizeZ + ").\n" + + "For a recast graph, the world-space tile size is defined as the cell size * the tile size in voxels"); + } + + var w = tileMeshes.tileRect.Width; + var h = tileMeshes.tileRect.Height; + UnityEngine.Assertions.Assert.AreEqual(w*h, tileMeshes.tileMeshes.Length); + + // Ensure the graph is large enough + var newTileBounds = IntRect.Union( + new IntRect(0, 0, tileXCount - 1, tileZCount - 1), + tileMeshes.tileRect + ); + Resize(newTileBounds); + tileMeshes.tileRect = tileMeshes.tileRect.Offset(-newTileBounds.Min); + + StartBatchTileUpdate(); + var updatedTiles = new NavmeshTile[w*h]; + for (int z = 0; z < h; z++) { + for (int x = 0; x < w; x++) { + var tile = tileMeshes.tileMeshes[x + z*w]; + + var offset = (Int3) new Vector3(0, yOffset, 0); + for (int i = 0; i < tile.verticesInTileSpace.Length; i++) { + tile.verticesInTileSpace[i] += offset; + } + var tileCoordinates = new Int2(x, z) + tileMeshes.tileRect.Min; + ReplaceTile(tileCoordinates.x, tileCoordinates.y, tile.verticesInTileSpace, tile.triangles); + updatedTiles[x + z*w] = GetTile(tileCoordinates.x, tileCoordinates.y); + } + } + EndBatchTileUpdate(); + + navmeshUpdateData.OnRecalculatedTiles(updatedTiles); + if (OnRecalculatedTiles != null) OnRecalculatedTiles(updatedTiles); + } + + protected override void PostDeserialization (GraphSerializationContext ctx) { + base.PostDeserialization(ctx); + if (ctx.meta.version < AstarSerializer.V4_3_80) { + // This field changed behavior in 4.3.80. This is an approximate (but very good) conversion. + collectionSettings.colliderRasterizeDetail = 2*cellSize*collectionSettings.colliderRasterizeDetail*collectionSettings.colliderRasterizeDetail/(math.PI*math.PI); + } + if (ctx.meta.version < AstarSerializer.V5_1_0) { + if (collectionSettings.tagMask.Count > 0 && collectionSettings.layerMask != -1) { + Debug.LogError("In version 5.1.0 or higher of the A* Pathfinding Project you can no longer include objects both using a tag mask and a layer mask. Please choose in the recast graph inspector which one you want to use."); + } else if (collectionSettings.tagMask.Count > 0) { + collectionSettings.collectionMode = CollectionSettings.FilterMode.Tags; + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/RecastGraph.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/RecastGraph.cs.meta new file mode 100644 index 0000000..7484327 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/RecastGraph.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: f840a2329e5964f00be5080879bae016 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities.meta new file mode 100644 index 0000000..8128266 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities.meta @@ -0,0 +1,2 @@ +fileFormatVersion: 2 +guid: e9fd53d051f6d4130872d5c2244b2fc6 diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs new file mode 100644 index 0000000..feb8fa7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs @@ -0,0 +1,442 @@ +#pragma warning disable 414 +using System.Collections.Generic; +using UnityEngine; +using Unity.Mathematics; +using Unity.Collections; +using Pathfinding.Util; + +namespace Pathfinding.Graphs.Util { + using Pathfinding.Drawing; + + public enum HeuristicOptimizationMode { + None, + Random, + RandomSpreadOut, + Custom + } + + /// + /// Implements heuristic optimizations. + /// + /// See: heuristic-opt + /// See: Game AI Pro - Pathfinding Architecture Optimizations by Steve Rabin and Nathan R. Sturtevant + /// + [System.Serializable] + public class EuclideanEmbedding { + /// + /// If heuristic optimization should be used and how to place the pivot points. + /// See: heuristic-opt + /// See: Game AI Pro - Pathfinding Architecture Optimizations by Steve Rabin and Nathan R. Sturtevant + /// + public HeuristicOptimizationMode mode; + + public int seed; + + /// All children of this transform will be used as pivot points + public Transform pivotPointRoot; + + public int spreadOutCount = 1; + + [System.NonSerialized] + public bool dirty; + + /// + /// Costs laid out as n*[int],n*[int],n*[int] where n is the number of pivot points. + /// Each node has n integers which is the cost from that node to the pivot node. + /// They are at around the same place in the array for simplicity and for cache locality. + /// + /// cost(nodeIndex, pivotIndex) = costs[nodeIndex*pivotCount+pivotIndex] + /// + public NativeArray costs { get; private set; } + public int pivotCount { get; private set; } + + GraphNode[] pivots; + + /* + * Seed for random number generator. + * Must not be zero + */ + const uint ra = 12820163; + + /* + * Seed for random number generator. + * Must not be zero + */ + const uint rc = 1140671485; + + /* + * Parameter for random number generator. + */ + uint rval; + + /// + /// Simple linear congruential generator. + /// See: http://en.wikipedia.org/wiki/Linear_congruential_generator + /// + uint GetRandom () { + rval = (ra*rval + rc); + return rval; + } + public void OnDisable () { + if (costs.IsCreated) costs.Dispose(); + costs = default; + pivotCount = 0; + } + + public static uint GetHeuristic (UnsafeSpan costs, uint pivotCount, uint nodeIndex1, uint nodeIndex2) { + uint mx = 0; + // TODO: Force pivotCount to be a multiple of 4 and use SIMD for performance + if (nodeIndex1 < costs.Length && nodeIndex2 < costs.Length) { + for (uint i = 0; i < pivotCount; i++) { + var c1 = costs[nodeIndex1*pivotCount+i]; + var c2 = costs[nodeIndex2*pivotCount+i]; + // If either of the nodes have an unknown cost to the pivot point, + // then we cannot use this pivot to calculate a heuristic + if (c1 == uint.MaxValue || c2 == uint.MaxValue) continue; + uint d = (uint)math.abs((int)c1 - (int)c2); + if (d > mx) mx = d; + } + } + return mx; + } + + void GetClosestWalkableNodesToChildrenRecursively (Transform tr, List nodes) { + foreach (Transform ch in tr) { + var info = AstarPath.active.GetNearest(ch.position, NNConstraint.Walkable); + if (info.node != null && info.node.Walkable) { + nodes.Add(info.node); + } + + GetClosestWalkableNodesToChildrenRecursively(ch, nodes); + } + } + + /// + /// Pick N random walkable nodes from all nodes in all graphs and add them to the buffer. + /// + /// Here we select N random nodes from a stream of nodes. + /// Probability of choosing the first N nodes is 1 + /// Probability of choosing node i is min(N/i,1) + /// A selected node will replace a random node of the previously + /// selected ones. + /// + /// See: https://en.wikipedia.org/wiki/Reservoir_sampling + /// + void PickNRandomNodes (int count, List buffer) { + int n = 0; + + var graphs = AstarPath.active.graphs; + + // Loop through all graphs + for (int j = 0; j < graphs.Length; j++) { + // Loop through all nodes in the graph + graphs[j].GetNodes(node => { + if (!node.Destroyed && node.Walkable) { + n++; + if ((GetRandom() % n) < count) { + if (buffer.Count < count) { + buffer.Add(node); + } else { + buffer[(int)(n%buffer.Count)] = node; + } + } + } + }); + } + } + + GraphNode PickAnyWalkableNode () { + var graphs = AstarPath.active.graphs; + GraphNode first = null; + + // Find any node in the graphs + for (int j = 0; j < graphs.Length; j++) { + graphs[j].GetNodes(node => { + if (node != null && node.Walkable && first == null) { + first = node; + } + }); + } + + return first; + } + + public void RecalculatePivots () { + if (mode == HeuristicOptimizationMode.None) { + pivotCount = 0; + pivots = null; + return; + } + + // Reset the random number generator + rval = (uint)seed; + + // Get a List from a pool + var pivotList = Pathfinding.Util.ListPool.Claim(); + + switch (mode) { + case HeuristicOptimizationMode.Custom: + if (pivotPointRoot == null) throw new System.Exception("heuristicOptimizationMode is HeuristicOptimizationMode.Custom, " + + "but no 'customHeuristicOptimizationPivotsRoot' is set"); + + GetClosestWalkableNodesToChildrenRecursively(pivotPointRoot, pivotList); + break; + case HeuristicOptimizationMode.Random: + PickNRandomNodes(spreadOutCount, pivotList); + break; + case HeuristicOptimizationMode.RandomSpreadOut: + if (pivotPointRoot != null) { + GetClosestWalkableNodesToChildrenRecursively(pivotPointRoot, pivotList); + } + + // If no pivot points were found, fall back to picking arbitrary nodes + if (pivotList.Count == 0) { + GraphNode first = PickAnyWalkableNode(); + + if (first != null) { + pivotList.Add(first); + } else { + Debug.LogError("Could not find any walkable node in any of the graphs."); + Pathfinding.Util.ListPool.Release(ref pivotList); + return; + } + } + + // Fill remaining slots with null + int toFill = spreadOutCount - pivotList.Count; + for (int i = 0; i < toFill; i++) pivotList.Add(null); + break; + default: + throw new System.Exception("Invalid HeuristicOptimizationMode: " + mode); + } + + pivots = pivotList.ToArray(); + + Pathfinding.Util.ListPool.Release(ref pivotList); + } + + class EuclideanEmbeddingSearchPath : Path { + public UnsafeSpan costs; + public uint costIndexStride; + public uint pivotIndex; + public GraphNode startNode; + public uint furthestNodeScore; + public GraphNode furthestNode; + + public static EuclideanEmbeddingSearchPath Construct (UnsafeSpan costs, uint costIndexStride, uint pivotIndex, GraphNode startNode) { + var p = PathPool.GetPath(); + p.costs = costs; + p.costIndexStride = costIndexStride; + p.pivotIndex = pivotIndex; + p.startNode = startNode; + p.furthestNodeScore = 0; + p.furthestNode = null; + return p; + } + + protected override void OnFoundEndNode (uint pathNode, uint hScore, uint gScore) { + throw new System.InvalidOperationException(); + } + + protected override void OnHeapExhausted () { + CompleteState = PathCompleteState.Complete; + } + + public override void OnVisitNode (uint pathNode, uint hScore, uint gScore) { + if (!pathHandler.IsTemporaryNode(pathNode)) { + // Get the node and then the node index from that. + // This is because a triangle mesh node will have 3 path nodes, + // but we want to collapse those to the same index as the original node. + var node = pathHandler.GetNode(pathNode); + uint baseIndex = node.NodeIndex*costIndexStride; + // EnsureCapacity(idx); + + costs[baseIndex + pivotIndex] = math.min(costs[baseIndex + pivotIndex], gScore); + + // Find the minimum distance from the node to all existing pivot points + uint mx = uint.MaxValue; + for (int p = 0; p <= pivotIndex; p++) mx = math.min(mx, costs[baseIndex + (uint)p]); + + // Pick the node which has the largest minimum distance to the existing pivot points + // (i.e pick the one furthest away from the existing ones) + if (mx > furthestNodeScore || furthestNode == null) { + furthestNodeScore = mx; + furthestNode = node; + } + } + } + + protected override void Prepare () { + pathHandler.AddTemporaryNode(new TemporaryNode { + associatedNode = startNode.NodeIndex, + position = startNode.position, + type = TemporaryNodeType.Start, + }); + heuristicObjective = new HeuristicObjective(0, Heuristic.None, 0.0f); + MarkNodesAdjacentToTemporaryEndNodes(); + AddStartNodesToHeap(); + } + } + + public void RecalculateCosts () { + if (pivots == null) RecalculatePivots(); + if (mode == HeuristicOptimizationMode.None) return; + + // Use a nested call to avoid allocating a delegate object + // even when we just do an early return. + RecalculateCostsInner(); + } + + void RecalculateCostsInner () { + pivotCount = 0; + + for (int i = 0; i < pivots.Length; i++) { + if (pivots[i] != null && (pivots[i].Destroyed || !pivots[i].Walkable)) { + throw new System.Exception("Invalid pivot nodes (destroyed or unwalkable)"); + } + } + + if (mode != HeuristicOptimizationMode.RandomSpreadOut) + for (int i = 0; i < pivots.Length; i++) + if (pivots[i] == null) + throw new System.Exception("Invalid pivot nodes (null)"); + + pivotCount = pivots.Length; + + System.Action startCostCalculation = null; + + int numComplete = 0; + + var nodeCount = AstarPath.active.nodeStorage.nextNodeIndex; + if (costs.IsCreated) costs.Dispose(); + // TODO: Quantize costs a bit to reduce memory usage? + costs = new NativeArray((int)nodeCount * pivotCount, Allocator.Persistent); + costs.AsUnsafeSpan().Fill(uint.MaxValue); + + startCostCalculation = (int pivotIndex) => { + GraphNode pivot = pivots[pivotIndex]; + + var path = EuclideanEmbeddingSearchPath.Construct( + costs.AsUnsafeSpan(), + (uint)pivotCount, + (uint)pivotIndex, + pivot + ); + + path.immediateCallback = (Path _) => { + if (mode == HeuristicOptimizationMode.RandomSpreadOut && pivotIndex < pivots.Length-1) { + // If the next pivot is null + // then find the node which is furthest away from the earlier + // pivot points + if (pivots[pivotIndex+1] == null) { + pivots[pivotIndex+1] = path.furthestNode; + + if (path.furthestNode == null) { + Debug.LogError("Failed generating random pivot points for heuristic optimizations"); + return; + } + } + + // Start next path + startCostCalculation(pivotIndex+1); + } + + numComplete++; + if (numComplete == pivotCount) { + // Last completed path + ApplyGridGraphEndpointSpecialCase(); + } + }; + + AstarPath.StartPath(path, true, true); + }; + + if (mode != HeuristicOptimizationMode.RandomSpreadOut) { + // All calculated in parallel + for (int i = 0; i < pivots.Length; i++) { + startCostCalculation(i); + } + } else { + // Recursive and serial + startCostCalculation(0); + } + + dirty = false; + } + + /// + /// Special case necessary for paths to unwalkable nodes right next to walkable nodes to be able to use good heuristics. + /// + /// This will find all unwalkable nodes in all grid graphs with walkable nodes as neighbours + /// and set the cost to reach them from each of the pivots as the minimum of the cost to + /// reach the neighbours of each node. + /// + /// See: ABPath.EndPointGridGraphSpecialCase + /// + void ApplyGridGraphEndpointSpecialCase () { + var costs = this.costs.AsUnsafeSpan(); +#if !ASTAR_NO_GRID_GRAPH + var graphs = AstarPath.active.graphs; + for (int i = 0; i < graphs.Length; i++) { + if (graphs[i] is GridGraph gg) { + // Found a grid graph + var nodes = gg.nodes; + + // Number of neighbours as an int + int mxnum = gg.neighbours == NumNeighbours.Four ? 4 : (gg.neighbours == NumNeighbours.Eight ? 8 : 6); + + for (int z = 0; z < gg.depth; z++) { + for (int x = 0; x < gg.width; x++) { + var node = nodes[z*gg.width + x]; + if (!node.Walkable) { + var pivotIndex = node.NodeIndex*(uint)pivotCount; + // Set all costs to reach this node to maximum + for (int piv = 0; piv < pivotCount; piv++) { + costs[pivotIndex + (uint)piv] = uint.MaxValue; + } + + // Loop through all potential neighbours of the node + // and set the cost to reach it as the minimum + // of the costs to reach any of the adjacent nodes + for (int d = 0; d < mxnum; d++) { + int nx, nz; + if (gg.neighbours == NumNeighbours.Six) { + // Hexagon graph + nx = x + GridGraph.neighbourXOffsets[GridGraph.hexagonNeighbourIndices[d]]; + nz = z + GridGraph.neighbourZOffsets[GridGraph.hexagonNeighbourIndices[d]]; + } else { + nx = x + GridGraph.neighbourXOffsets[d]; + nz = z + GridGraph.neighbourZOffsets[d]; + } + + // Check if the position is still inside the grid + if (nx >= 0 && nz >= 0 && nx < gg.width && nz < gg.depth) { + var adjacentNode = gg.nodes[nz*gg.width + nx]; + if (adjacentNode.Walkable) { + for (uint piv = 0; piv < pivotCount; piv++) { + uint cost = costs[adjacentNode.NodeIndex*(uint)pivotCount + piv] + gg.neighbourCosts[d]; + costs[pivotIndex + piv] = System.Math.Min(costs[pivotIndex + piv], cost); + //Debug.DrawLine((Vector3)node.position, (Vector3)adjacentNode.position, Color.blue, 1); + } + } + } + } + } + } + } + } + } +#endif + } + + public void OnDrawGizmos () { + if (pivots != null) { + for (int i = 0; i < pivots.Length; i++) { + if (pivots[i] != null && !pivots[i].Destroyed) { + Draw.SolidBox((Vector3)pivots[i].position, Vector3.one, new Color(159/255.0f, 94/255.0f, 194/255.0f, 0.8f)); + } + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs.meta new file mode 100644 index 0000000..dd2f06c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/EuclideanEmbedding.cs.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 3ac3213e3eeb14eef91939f5281682e6 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs new file mode 100644 index 0000000..3e25b20 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs @@ -0,0 +1,575 @@ +using Unity.Mathematics; +using UnityEngine; + +namespace Pathfinding.Util { + /// + /// Transforms to and from world space to a 2D movement plane. + /// The transformation is guaranteed to be purely a rotation + /// so no scale or offset is used. This interface is primarily + /// used to make it easier to write movement scripts which can + /// handle movement both in the XZ plane and in the XY plane. + /// + /// See: + /// + public interface IMovementPlane { + Vector2 ToPlane(Vector3 p); + Vector2 ToPlane(Vector3 p, out float elevation); + Vector3 ToWorld(Vector2 p, float elevation = 0); + SimpleMovementPlane ToSimpleMovementPlane(); + } + + /// + /// A matrix wrapper which can be used to project points from world space to a movement plane. + /// + /// In contrast to , this is represented by a matrix instead of a quaternion. + /// This means it is less space efficient (36 bytes instead of 16 bytes) but it is more performant when + /// you need to do a lot of ToPlane conversions. + /// + public readonly struct ToPlaneMatrix { + public readonly float3x3 matrix; + + public ToPlaneMatrix (NativeMovementPlane plane) => this.matrix = new float3x3(math.conjugate(plane.rotation)); + + /// + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// See: + /// + public float2 ToPlane(float3 p) => math.mul(matrix, p).xz; + + /// + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// The elevation coordinate will be returned as the y coordinate of the returned vector. + /// + /// See: + /// + public float3 ToXZPlane(float3 p) => math.mul(matrix, p); + + /// + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// See: + /// + public float2 ToPlane (float3 p, out float elevation) { + var v = math.mul(matrix, p); + elevation = v.y; + return v.xz; + } + } + + /// + /// A matrix wrapper which can be used to project points from a movement plane to world space. + /// + /// In contrast to , this is represented by a matrix instead of a quaternion. + /// This means it is less space efficient (36 bytes instead of 16 bytes) but it is more performant when + /// you need to do a lot of ToWorld conversions. + /// + public readonly struct ToWorldMatrix { + public readonly float3x3 matrix; + + public ToWorldMatrix (NativeMovementPlane plane) => this.matrix = new float3x3(plane.rotation); + + public ToWorldMatrix (float3x3 matrix) => this.matrix = matrix; + + public float3 ToWorld(float2 p, float elevation = 0) => math.mul(matrix, new float3(p.x, elevation, p.y)); + + /// + /// Transforms a bounding box from local space to world space. + /// + /// The Y coordinate of the bounding box is the elevation coordinate. + /// + /// See: https://zeux.io/2010/10/17/aabb-from-obb-with-component-wise-abs/ + /// + public Bounds ToWorld (Bounds bounds) { + Bounds result = default; + result.center = math.mul(matrix, (float3)bounds.center); + result.extents = math.mul(new float3x3( + math.abs(matrix.c0), + math.abs(matrix.c1), + math.abs(matrix.c2) + ), (float3)bounds.extents); + return result; + } + } + + /// A variant of that can be passed to burst functions + public readonly struct NativeMovementPlane { + /// + /// The rotation of the plane. + /// The plane is defined by the XZ-plane rotated by this quaternion. + /// + /// Should always be normalized. + /// + public readonly quaternion rotation; + + /// Normal of the plane + // TODO: Check constructor for float3x3(quaternion), seems smarter, at least in burst + public float3 up => 2 * new float3(rotation.value.x * rotation.value.y - rotation.value.w * rotation.value.z, 0.5f - rotation.value.x * rotation.value.x - rotation.value.z * rotation.value.z, rotation.value.w * rotation.value.x + rotation.value.y * rotation.value.z); // math.mul(rotation, Vector3.up); + + public NativeMovementPlane(quaternion rotation) { + // We need to normalize to make sure that math.inverse(rotation) == math.conjugate(rotation). + // We want to use conjugate because it's faster. + this.rotation = math.normalizesafe(rotation); + } + + public NativeMovementPlane(SimpleMovementPlane plane) : this(plane.rotation) {} + + public ToPlaneMatrix AsWorldToPlaneMatrix() => new ToPlaneMatrix(this); + public ToWorldMatrix AsPlaneToWorldMatrix() => new ToWorldMatrix(this); + + public float ProjectedLength(float3 v) => math.length(ToPlane(v)); + + /// + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// For a graph rotated with the rotation (-90, 0, 0) this will transform + /// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0) + /// this will tranform a coordinate (x,y,z) to (x,z). More generally for + /// a graph with a quaternion rotation R this will transform a vector V + /// to inverse(R) * V (i.e rotate the vector V using the inverse of rotation R). + /// + public float2 ToPlane (float3 p) { + return math.mul(math.conjugate(rotation), p).xz; + } + + /// Transforms from world space to the 'ground' plane of the graph + public float2 ToPlane (float3 p, out float elevation) { + p = math.mul(math.conjugate(rotation), p); + elevation = p.y; + return p.xz; + } + + /// + /// Transforms from the 'ground' plane of the graph to world space. + /// The transformation is purely a rotation so no scale or offset is used. + /// + public float3 ToWorld (float2 p, float elevation = 0f) { + return math.mul(rotation, new float3(p.x, elevation, p.y)); + } + + /// + /// Projects a rotation onto the plane. + /// + /// The returned angle is such that + /// + /// + /// var angle = ...; + /// var q = math.mul(plane.rotation, quaternion.RotateY(angle)); + /// AstarMath.DeltaAngle(plane.ToPlane(q), -angle) == 0; // or at least approximately equal + /// + /// + /// See: + /// See: + /// + /// the rotation to project + public float ToPlane (quaternion rotation) { + var inPlaneRotation = math.mul(math.conjugate(this.rotation), rotation); + // Ensure the rotation axis is always along +Y + if (inPlaneRotation.value.y < 0) inPlaneRotation.value = -inPlaneRotation.value; + var twist = math.normalizesafe(new quaternion(0, inPlaneRotation.value.y, 0, inPlaneRotation.value.w)); + return -VectorMath.QuaternionAngle(twist); + } + + public quaternion ToWorldRotation (float angle) { + return math.mul(rotation, quaternion.RotateY(-angle)); + } + + public quaternion ToWorldRotationDelta (float deltaAngle) { + return quaternion.AxisAngle(ToWorld(float2.zero, 1), -deltaAngle); + } + + /// + /// Transforms a bounding box from local space to world space. + /// + /// The Y coordinate of the bounding box is the elevation coordinate. + /// + public Bounds ToWorld(Bounds bounds) => AsPlaneToWorldMatrix().ToWorld(bounds); + } + + /// + /// Represents the orientation of a plane. + /// + /// When a character walks around in the world, it may not necessarily walk on the XZ-plane. + /// It may be the case that the character is on a spherical world, or maybe it walks on a wall or upside down on the ceiling. + /// + /// A movement plane is used to handle this. It contains functions for converting a 3D point into a 2D point on that plane, and functions for converting back to 3D. + /// + /// See: NativeMovementPlane + /// +#if MODULE_COLLECTIONS_2_0_0_OR_NEWER && UNITY_2022_2_OR_NEWER + [Unity.Collections.GenerateTestsForBurstCompatibility] +#endif + public readonly struct SimpleMovementPlane : IMovementPlane { + public readonly Quaternion rotation; + public readonly Quaternion inverseRotation; + readonly byte plane; + public bool isXY => plane == 1; + public bool isXZ => plane == 2; + + /// A plane that spans the X and Y axes + public static readonly SimpleMovementPlane XYPlane = new SimpleMovementPlane(Quaternion.Euler(-90, 0, 0)); + + /// A plane that spans the X and Z axes + public static readonly SimpleMovementPlane XZPlane = new SimpleMovementPlane(Quaternion.identity); + + public SimpleMovementPlane (Quaternion rotation) { + this.rotation = rotation; + // TODO: Normalize #rotation and compute inverse every time instead (less memory) + inverseRotation = Quaternion.Inverse(rotation); + // Some short circuiting code for the movement plane calculations + if (rotation == XYPlane.rotation) plane = 1; + else if (rotation == Quaternion.identity) plane = 2; + else plane = 0; + } + + /// + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// For a graph rotated with the rotation (-90, 0, 0) this will transform + /// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0) + /// this will tranform a coordinate (x,y,z) to (x,z). More generally for + /// a graph with a quaternion rotation R this will transform a vector V + /// to inverse(R) * V (i.e rotate the vector V using the inverse of rotation R). + /// + public Vector2 ToPlane (Vector3 point) { + // These special cases cover most graph orientations used in practice. + // Having them here improves performance in those cases by a factor of + // 2.5 without impacting the generic case in any significant way. + if (isXY) return new Vector2(point.x, point.y); + if (!isXZ) point = inverseRotation * point; + return new Vector2(point.x, point.z); + } + + /// + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// For a graph rotated with the rotation (-90, 0, 0) this will transform + /// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0) + /// this will tranform a coordinate (x,y,z) to (x,z). More generally for + /// a graph with a quaternion rotation R this will transform a vector V + /// to inverse(R) * V (i.e rotate the vector V using the inverse of rotation R). + /// + public float2 ToPlane (float3 point) { + return ((float3)(inverseRotation * (Vector3)point)).xz; + } + + /// + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + public Vector2 ToPlane (Vector3 point, out float elevation) { + if (!isXZ) point = inverseRotation * point; + elevation = point.y; + return new Vector2(point.x, point.z); + } + + /// + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + public float2 ToPlane (float3 point, out float elevation) { + point = math.mul(inverseRotation, point); + elevation = point.y; + return point.xz; + } + + /// + /// Transforms from the 'ground' plane of the graph to world space. + /// The transformation is purely a rotation so no scale or offset is used. + /// + public Vector3 ToWorld (Vector2 point, float elevation = 0) { + return rotation * new Vector3(point.x, elevation, point.y); + } + + /// + /// Transforms from the 'ground' plane of the graph to world space. + /// The transformation is purely a rotation so no scale or offset is used. + /// + public float3 ToWorld (float2 point, float elevation = 0) { + return rotation * new Vector3(point.x, elevation, point.y); + } + + public SimpleMovementPlane ToSimpleMovementPlane () { + return this; + } + + public static bool operator== (SimpleMovementPlane lhs, SimpleMovementPlane rhs) { + return lhs.rotation == rhs.rotation; + } + + public static bool operator!= (SimpleMovementPlane lhs, SimpleMovementPlane rhs) { + return lhs.rotation != rhs.rotation; + } + + public override bool Equals (System.Object other) { + if (!(other is SimpleMovementPlane)) return false; + return rotation == ((SimpleMovementPlane)other).rotation; + } + + public override int GetHashCode () { + return rotation.GetHashCode(); + } + } + + /// Generic 3D coordinate transformation + public interface ITransform { + Vector3 Transform(Vector3 position); + Vector3 InverseTransform(Vector3 position); + } + + /// Like , but mutable + public class MutableGraphTransform : GraphTransform { + public MutableGraphTransform (Matrix4x4 matrix) : base(matrix) {} + + /// Replace this transform with the given matrix transformation + public void SetMatrix (Matrix4x4 matrix) { + Set(matrix); + } + } + + /// + /// Defines a transformation from graph space to world space. + /// This is essentially just a simple wrapper around a matrix, but it has several utilities that are useful. + /// + public class GraphTransform : IMovementPlane, ITransform { + /// True if this transform is the identity transform (i.e it does not do anything) + public bool identity { get { return isIdentity; } } + + /// True if this transform is a pure translation without any scaling or rotation + public bool onlyTranslational { get { return isOnlyTranslational; } } + + bool isXY; + bool isXZ; + bool isOnlyTranslational; + bool isIdentity; + + public Matrix4x4 matrix { get; private set; } + public Matrix4x4 inverseMatrix { get; private set; } + Vector3 up; + Vector3 translation; + Int3 i3translation; + public Quaternion rotation { get; private set; } + Quaternion inverseRotation; + + public static readonly GraphTransform identityTransform = new GraphTransform(Matrix4x4.identity); + + /// Transforms from the XZ plane to the XY plane + public static readonly GraphTransform xyPlane = new GraphTransform(Matrix4x4.TRS(Vector3.zero, Quaternion.Euler(-90, 0, 0), Vector3.one)); + + /// Transforms from the XZ plane to the XZ plane (i.e. an identity transformation) + public static readonly GraphTransform xzPlane = new GraphTransform(Matrix4x4.identity); + + public GraphTransform (Matrix4x4 matrix) { + Set(matrix); + } + + protected void Set (Matrix4x4 matrix) { + this.matrix = matrix; + inverseMatrix = matrix.inverse; + isIdentity = matrix.isIdentity; + isOnlyTranslational = MatrixIsTranslational(matrix); + up = matrix.MultiplyVector(Vector3.up).normalized; + translation = matrix.MultiplyPoint3x4(Vector3.zero); + i3translation = (Int3)translation; + + // Extract the rotation from the matrix. This is only correct if the matrix has no skew, but we only + // want to use it for the movement plane so as long as the Up axis is parpendicular to the Forward + // axis everything should be ok. In fact the only case in the project when all three axes are not + // perpendicular is when hexagon or isometric grid graphs are used, but in those cases only the + // X and Z axes are not perpendicular. + rotation = Quaternion.LookRotation(TransformVector(Vector3.forward), TransformVector(Vector3.up)); + inverseRotation = Quaternion.Inverse(rotation); + // Some short circuiting code for the movement plane calculations + isXY = rotation == Quaternion.Euler(-90, 0, 0); + isXZ = rotation == Quaternion.Euler(0, 0, 0); + } + + public Vector3 WorldUpAtGraphPosition (Vector3 point) { + return up; + } + + static bool MatrixIsTranslational (Matrix4x4 matrix) { + return matrix.GetColumn(0) == new Vector4(1, 0, 0, 0) && matrix.GetColumn(1) == new Vector4(0, 1, 0, 0) && matrix.GetColumn(2) == new Vector4(0, 0, 1, 0) && matrix.m33 == 1; + } + + public Vector3 Transform (Vector3 point) { + if (onlyTranslational) return point + translation; + return matrix.MultiplyPoint3x4(point); + } + + public Vector3 TransformVector (Vector3 dir) { + if (onlyTranslational) return dir; + return matrix.MultiplyVector(dir); + } + + public void Transform (Int3[] arr) { + if (onlyTranslational) { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] += i3translation; + } else { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)matrix.MultiplyPoint3x4((Vector3)arr[i]); + } + } + + public void Transform (UnsafeSpan arr) { + if (onlyTranslational) { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] += i3translation; + } else { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)matrix.MultiplyPoint3x4((Vector3)arr[i]); + } + } + + public void Transform (Vector3[] arr) { + if (onlyTranslational) { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] += translation; + } else { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] = matrix.MultiplyPoint3x4(arr[i]); + } + } + + public Vector3 InverseTransform (Vector3 point) { + if (onlyTranslational) return point - translation; + return inverseMatrix.MultiplyPoint3x4(point); + } + + public Vector3 InverseTransformVector (Vector3 dir) { + if (onlyTranslational) return dir; + return inverseMatrix.MultiplyVector(dir); + } + + public Int3 InverseTransform (Int3 point) { + if (onlyTranslational) return point - i3translation; + return (Int3)inverseMatrix.MultiplyPoint3x4((Vector3)point); + } + + public void InverseTransform (Int3[] arr) { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)inverseMatrix.MultiplyPoint3x4((Vector3)arr[i]); + } + + public void InverseTransform (UnsafeSpan arr) { + for (int i = arr.Length - 1; i >= 0; i--) arr[i] = (Int3)inverseMatrix.MultiplyPoint3x4((Vector3)arr[i]); + } + + public static GraphTransform operator * (GraphTransform lhs, Matrix4x4 rhs) { + return new GraphTransform(lhs.matrix * rhs); + } + + public static GraphTransform operator * (Matrix4x4 lhs, GraphTransform rhs) { + return new GraphTransform(lhs * rhs.matrix); + } + + public Bounds Transform (Bounds bounds) { + if (onlyTranslational) return new Bounds(bounds.center + translation, bounds.size); + + var corners = ArrayPool.Claim(8); + var extents = bounds.extents; + corners[0] = Transform(bounds.center + new Vector3(extents.x, extents.y, extents.z)); + corners[1] = Transform(bounds.center + new Vector3(extents.x, extents.y, -extents.z)); + corners[2] = Transform(bounds.center + new Vector3(extents.x, -extents.y, extents.z)); + corners[3] = Transform(bounds.center + new Vector3(extents.x, -extents.y, -extents.z)); + corners[4] = Transform(bounds.center + new Vector3(-extents.x, extents.y, extents.z)); + corners[5] = Transform(bounds.center + new Vector3(-extents.x, extents.y, -extents.z)); + corners[6] = Transform(bounds.center + new Vector3(-extents.x, -extents.y, extents.z)); + corners[7] = Transform(bounds.center + new Vector3(-extents.x, -extents.y, -extents.z)); + + var min = corners[0]; + var max = corners[0]; + for (int i = 1; i < 8; i++) { + min = Vector3.Min(min, corners[i]); + max = Vector3.Max(max, corners[i]); + } + ArrayPool.Release(ref corners); + return new Bounds((min+max)*0.5f, max - min); + } + + public Bounds InverseTransform (Bounds bounds) { + if (onlyTranslational) return new Bounds(bounds.center - translation, bounds.size); + + var corners = ArrayPool.Claim(8); + var extents = bounds.extents; + corners[0] = InverseTransform(bounds.center + new Vector3(extents.x, extents.y, extents.z)); + corners[1] = InverseTransform(bounds.center + new Vector3(extents.x, extents.y, -extents.z)); + corners[2] = InverseTransform(bounds.center + new Vector3(extents.x, -extents.y, extents.z)); + corners[3] = InverseTransform(bounds.center + new Vector3(extents.x, -extents.y, -extents.z)); + corners[4] = InverseTransform(bounds.center + new Vector3(-extents.x, extents.y, extents.z)); + corners[5] = InverseTransform(bounds.center + new Vector3(-extents.x, extents.y, -extents.z)); + corners[6] = InverseTransform(bounds.center + new Vector3(-extents.x, -extents.y, extents.z)); + corners[7] = InverseTransform(bounds.center + new Vector3(-extents.x, -extents.y, -extents.z)); + + var min = corners[0]; + var max = corners[0]; + for (int i = 1; i < 8; i++) { + min = Vector3.Min(min, corners[i]); + max = Vector3.Max(max, corners[i]); + } + ArrayPool.Release(ref corners); + return new Bounds((min+max)*0.5f, max - min); + } + + #region IMovementPlane implementation + + /// + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + /// For a graph rotated with the rotation (-90, 0, 0) this will transform + /// a coordinate (x,y,z) to (x,y). For a graph with the rotation (0,0,0) + /// this will tranform a coordinate (x,y,z) to (x,z). More generally for + /// a graph with a quaternion rotation R this will transform a vector V + /// to R * V (i.e rotate the vector V using the rotation R). + /// + Vector2 IMovementPlane.ToPlane (Vector3 point) { + // These special cases cover most graph orientations used in practice. + // Having them here improves performance in those cases by a factor of + // 2.5 without impacting the generic case in any significant way. + if (isXY) return new Vector2(point.x, point.y); + if (!isXZ) point = inverseRotation * point; + return new Vector2(point.x, point.z); + } + + /// + /// Transforms from world space to the 'ground' plane of the graph. + /// The transformation is purely a rotation so no scale or offset is used. + /// + Vector2 IMovementPlane.ToPlane (Vector3 point, out float elevation) { + if (!isXZ) point = inverseRotation * point; + elevation = point.y; + return new Vector2(point.x, point.z); + } + + /// + /// Transforms from the 'ground' plane of the graph to world space. + /// The transformation is purely a rotation so no scale or offset is used. + /// + Vector3 IMovementPlane.ToWorld (Vector2 point, float elevation) { + return rotation * new Vector3(point.x, elevation, point.y); + } + + public SimpleMovementPlane ToSimpleMovementPlane () { + return new SimpleMovementPlane(rotation); + } + + #endregion + + /// Copies the data in this transform to another mutable graph transform + public void CopyTo (MutableGraphTransform graphTransform) { + graphTransform.isXY = isXY; + graphTransform.isXZ = isXZ; + graphTransform.isOnlyTranslational = isOnlyTranslational; + graphTransform.isIdentity = isIdentity; + graphTransform.matrix = matrix; + graphTransform.inverseMatrix = inverseMatrix; + graphTransform.up = up; + graphTransform.translation = translation; + graphTransform.i3translation = i3translation; + graphTransform.rotation = rotation; + graphTransform.inverseRotation = inverseRotation; + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs.meta new file mode 100644 index 0000000..d9d146c --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GraphTransform.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: f9d3961465175430a84fd52d1bd31b05 +timeCreated: 1474479722 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs new file mode 100644 index 0000000..f412ab7 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs @@ -0,0 +1,220 @@ +using System.Collections.Generic; + +namespace Pathfinding.Graphs.Util { + /// + /// Holds a lookup datastructure to quickly find objects inside rectangles. + /// Objects of type T occupy an integer rectangle in the grid and they can be + /// moved efficiently. You can query for all objects that touch a specified + /// rectangle that runs in O(m*k+r) time where m is the number of objects that + /// the query returns, k is the average number of cells that an object + /// occupies and r is the area of the rectangle query. + /// + /// All objects must be contained within a rectangle with one point at the origin + /// (inclusive) and one at (exclusive) that is specified in the constructor. + /// + public class GridLookup where T : class { + Int2 size; + Item[] cells; + /// + /// Linked list of all items. + /// Note that the first item in the list is a dummy item and does not contain any data. + /// + Root all = new Root(); + Dictionary rootLookup = new Dictionary(); + Stack itemPool = new Stack(); + + public GridLookup (Int2 size) { + this.size = size; + cells = new Item[size.x*size.y]; + for (int i = 0; i < cells.Length; i++) cells[i] = new Item(); + } + + internal class Item { + public Root root; + public Item prev, next; + } + + public class Root { + /// Underlying object + public T obj; + /// Next item in the linked list of all roots + public Root next; + /// Previous item in the linked list of all roots + internal Root prev; + internal IntRect previousBounds = new IntRect(0, 0, -1, -1); + /// References to an item in each grid cell that this object is contained inside + internal List items = new List(); + internal bool flag; + + public UnityEngine.Vector3 previousPosition = new UnityEngine.Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + public UnityEngine.Quaternion previousRotation; + } + + /// Linked list of all items + public Root AllItems { + get { + return all.next; + } + } + + public void Clear () { + rootLookup.Clear(); + all.next = null; + foreach (var item in cells) item.next = null; + } + + public Root GetRoot (T item) { + rootLookup.TryGetValue(item, out Root root); + return root; + } + + /// + /// Add an object to the lookup data structure. + /// Returns: A handle which can be used for Move operations + /// + public Root Add (T item, IntRect bounds) { + var root = new Root { + obj = item, + prev = all, + next = all.next + }; + + all.next = root; + if (root.next != null) root.next.prev = root; + + rootLookup.Add(item, root); + Move(item, bounds); + return root; + } + + /// Removes an item from the lookup data structure + public void Remove (T item) { + if (!rootLookup.TryGetValue(item, out Root root)) { + return; + } + + // Make the item occupy no cells at all + Move(item, new IntRect(0, 0, -1, -1)); + rootLookup.Remove(item); + root.prev.next = root.next; + if (root.next != null) root.next.prev = root.prev; + } + + public void Dirty (T item) { + if (!rootLookup.TryGetValue(item, out Root root)) { + return; + } + + root.previousPosition = new UnityEngine.Vector3(float.PositiveInfinity, float.PositiveInfinity, float.PositiveInfinity); + } + + /// Move an object to occupy a new set of cells + public void Move (T item, IntRect bounds) { + if (!rootLookup.TryGetValue(item, out Root root)) { + throw new System.ArgumentException("The item has not been added to this object"); + } + + var prev = root.previousBounds; + if (prev == bounds) return; + + // Remove all + for (int i = 0; i < root.items.Count; i++) { + Item ob = root.items[i]; + ob.prev.next = ob.next; + if (ob.next != null) ob.next.prev = ob.prev; + } + + root.previousBounds = bounds; + int reusedItems = 0; + for (int z = bounds.ymin; z <= bounds.ymax; z++) { + for (int x = bounds.xmin; x <= bounds.xmax; x++) { + Item ob; + if (reusedItems < root.items.Count) { + ob = root.items[reusedItems]; + } else { + ob = itemPool.Count > 0 ? itemPool.Pop() : new Item(); + ob.root = root; + root.items.Add(ob); + } + reusedItems++; + + ob.prev = cells[x + z*size.x]; + ob.next = ob.prev.next; + ob.prev.next = ob; + if (ob.next != null) ob.next.prev = ob; + } + } + + for (int i = root.items.Count-1; i >= reusedItems; i--) { + Item ob = root.items[i]; + ob.root = null; + ob.next = null; + ob.prev = null; + root.items.RemoveAt(i); + itemPool.Push(ob); + } + } + + /// + /// Returns all objects of a specific type inside the cells marked by the rectangle. + /// Note: For better memory usage, consider pooling the list using Pathfinding.Util.ListPool after you are done with it + /// + public List QueryRect(IntRect r) where U : class, T { + List result = Pathfinding.Util.ListPool.Claim(); + + // Loop through tiles and check which objects are inside them + for (int z = r.ymin; z <= r.ymax; z++) { + var zs = z*size.x; + for (int x = r.xmin; x <= r.xmax; x++) { + Item c = cells[x + zs]; + // Note, first item is a dummy, so it is ignored + while (c.next != null) { + c = c.next; + var obj = c.root.obj as U; + if (!c.root.flag && obj != null) { + c.root.flag = true; + result.Add(obj); + } + } + } + } + + // Reset flags + for (int z = r.ymin; z <= r.ymax; z++) { + var zs = z*size.x; + for (int x = r.xmin; x <= r.xmax; x++) { + Item c = cells[x + zs]; + while (c.next != null) { + c = c.next; + c.root.flag = false; + } + } + } + + return result; + } + + public void Resize (IntRect newBounds) { + var newCells = new Item[newBounds.Width * newBounds.Height]; + for (int z = 0; z < size.y; z++) { + for (int x = 0; x < size.x; x++) { + if (newBounds.Contains(x, z)) { + newCells[(x - newBounds.xmin) + (z - newBounds.ymin) * newBounds.Width] = cells[x + z*size.x]; + } + } + } + for (int i = 0; i < newCells.Length; i++) { + if (newCells[i] == null) newCells[i] = new Item(); + } + this.size = new Int2(newBounds.Width, newBounds.Height); + this.cells = newCells; + var root = this.AllItems; + var offset = new Int2(-newBounds.xmin, -newBounds.ymin); + var bounds = new IntRect(0, 0, newBounds.Width - 1, newBounds.Height - 1); + while (root != null) { + root.previousBounds = IntRect.Intersection(root.previousBounds.Offset(offset), bounds); + root = root.next; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs.meta new file mode 100644 index 0000000..2d2d1f0 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/GridLookup.cs.meta @@ -0,0 +1,12 @@ +fileFormatVersion: 2 +guid: 7b09e5cbe5d4644c2b4ed9eed14cc13a +timeCreated: 1475417043 +licenseType: Store +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs new file mode 100644 index 0000000..3b6ed49 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs @@ -0,0 +1,4 @@ + +// This file has been removed from the project. Since UnityPackages cannot +// delete files, only replace them, this message is left here to prevent old +// files from causing compiler errors diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs.meta new file mode 100644 index 0000000..b43caee --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/NavMeshRenderer.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: b56789f958bf1496ba91f7e2b4147166 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs new file mode 100644 index 0000000..a2f1ff9 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs @@ -0,0 +1,311 @@ +using UnityEngine; +using System.Collections.Generic; +using Pathfinding.Graphs.Navmesh; +using Pathfinding.Util; +using Pathfinding.Drawing; + +namespace Pathfinding { + /// + /// Explicit mesh object for recast graphs. + /// + /// Sometimes you want to tweak the navmesh on a per-object basis. For example you might want to make some objects completely unwalkable, or you might want to special case some objects to remove them from the navmesh altogether. + /// + /// You can do this using the component. Attach it to any object you want to modify and configure the settings as you wish. + /// + /// Using the component you can: + /// + /// - Exclude an object from the graph completely. + /// - Make the surfaces of an object unwalkable. + /// - Make the surfaces of an object walkable (this is just the default behavior). + /// - Create seams in the navmesh between adjacent objects. + /// - Mark the surfaces of an object with a specific tag (see tags) (view in online documentation for working links). + /// + /// Adding this component to an object will make sure it is included in any recast graphs. + /// It will be included even if the Rasterize Meshes toggle is set to false. + /// + /// Using RecastMeshObjs instead of relying on the Rasterize Meshes option is good for several reasons. + /// - Rasterize Meshes is slow. If you are using a tiled graph and you are updating it, every time something is recalculated + /// the graph will have to search all meshes in your scene for ones to rasterize. In contrast, RecastMeshObjs are stored + /// in a tree for extremely fast lookup (O(log n + k) compared to O(n) where n is the number of meshes in your scene and k is the number of meshes + /// which should be rasterized, if you know Big-O notation). + /// - The RecastMeshObj exposes some options which can not be accessed using the Rasterize Meshes toggle. See member documentation for more info. + /// This can for example be used to include meshes in the recast graph rasterization, but make sure that the character cannot walk on them. + /// + /// Since the objects are stored in a tree, and trees are slow to update, there is an enforcement that objects are not allowed to move + /// unless the option is enabled. When the dynamic option is enabled, the object will be stored in an array instead of in the tree. + /// This will reduce the performance improvement over 'Rasterize Meshes' but is still faster. + /// + /// If a mesh filter and a mesh renderer is attached to this GameObject, those will be used in the rasterization + /// otherwise if a collider is attached, that will be used. + /// + [AddComponentMenu("Pathfinding/Navmesh/RecastMeshObj")] + [DisallowMultipleComponent] + [HelpURL("https://arongranberg.com/astar/documentation/stable/recastmeshobj.html")] + public class RecastMeshObj : VersionedMonoBehaviour { + /// Components are stored in a tree for fast lookups + protected static AABBTree tree = new AABBTree(); + + /// + /// Enable if the object will move during runtime. + /// + /// If disabled, the object will be assumed to stay in the same position, and keep the same size, until the component is disabled or destroyed. + /// + /// Disabling this will provide a small performance boost when doing graph updates, + /// as the graph no longer has to check if this RecastMeshObj might have moved. + /// + /// Even you set dynamic=false, you can disable the component, move the object, and enable it at the new position. + /// + public bool dynamic = true; + + /// + /// 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. + /// + /// If you enable this on a mesh that is actually hollow then the hollow region will also be treated as unwalkable. + /// + public bool solid = false; + + /// Source of geometry when voxelizing this object + public GeometrySource geometrySource = GeometrySource.Auto; + + /// + /// Determines if the object should be included in scans or not. + /// See: + /// + public ScanInclusion includeInScan = ScanInclusion.Auto; + + public enum ScanInclusion { + /// + /// Includes or excludes the object as normal based on the recast graph's layer mask and tag mask. + /// + /// See: + /// + Auto, + /// This object will be completely ignored by the graph + AlwaysExclude, + /// This object will always be included when scanning a recast graph, even if it would normally be filtered out + AlwaysInclude, + } + + /// Source of geometry when voxelizing this object + public enum GeometrySource { + /// Uses the MeshFilter component on this GameObject if available, otherwise uses the collider + Auto, + /// Always uses the MeshFilter component on this GameObject + MeshFilter, + /// Always uses the Collider on this GameObject + Collider, + } + + public enum Mode { + /// All surfaces on this mesh will be made unwalkable + UnwalkableSurface = 1, + /// All surfaces on this mesh will be walkable + WalkableSurface, + /// All surfaces on this mesh will be walkable and a seam will be created between the surfaces on this mesh and the surfaces on other meshes (with a different surface id) + WalkableSurfaceWithSeam, + /// All surfaces on this mesh will be walkable and the nodes will be given the specified tag. A seam will be created between the surfaces on this mesh and the surfaces on other meshes (with a different tag or surface id) + WalkableSurfaceWithTag, + } + + /// + /// Voxel area for mesh. + /// This area (not to be confused with pathfinding areas, this is only used when rasterizing meshes for the recast graph) field + /// can be used to explicitly insert edges in the navmesh geometry or to make some parts of the mesh unwalkable. + /// + /// When rasterizing the world and two objects with different surface id values are adjacent to each other, a split in the navmesh geometry + /// will be added between them, characters will still be able to walk between them, but this can be useful when working with navmesh updates. + /// + /// Navmesh updates which recalculate a whole tile (updatePhysics=True) are very slow So if there are special places + /// which you know are going to be updated quite often, for example at a door opening (opened/closed door) you + /// can use surface IDs to create splits on the navmesh for easier updating using normal graph updates (updatePhysics=False). + /// See the below video for more information. + /// + /// Video: https://www.youtube.com/watch?v=CS6UypuEMwM + /// + /// Deprecated: Use and instead + /// + [System.Obsolete("Use mode and surfaceID instead")] + public int area { + get { + switch (mode) { + case Mode.UnwalkableSurface: + return -1; + case Mode.WalkableSurface: + default: + return 0; + case Mode.WalkableSurfaceWithSeam: + return surfaceID; + case Mode.WalkableSurfaceWithTag: + return surfaceID; + } + } + set { + if (value <= -1) mode = Mode.UnwalkableSurface; + if (value == 0) mode = Mode.WalkableSurface; + if (value > 0) { + mode = Mode.WalkableSurfaceWithSeam; + surfaceID = value; + } + } + } + + /// + /// Voxel area for mesh. + /// This area (not to be confused with pathfinding areas, this is only used when rasterizing meshes for the recast graph) field + /// can be used to explicitly insert edges in the navmesh geometry or to make some parts of the mesh unwalkable. + /// + /// When rasterizing the world and two objects with different surface id values are adjacent to each other, a split in the navmesh geometry + /// will be added between them, characters will still be able to walk between them, but this can be useful when working with navmesh updates. + /// + /// Navmesh updates which recalculate a whole tile (updatePhysics=True) are very slow So if there are special places + /// which you know are going to be updated quite often, for example at a door opening (opened/closed door) you + /// can use surface IDs to create splits on the navmesh for easier updating using normal graph updates (updatePhysics=False). + /// See the below video for more information. + /// + /// Video: https://www.youtube.com/watch?v=CS6UypuEMwM + /// + /// When is set to Mode.WalkableSurfaceWithTag then this value will be interpreted as a pathfinding tag. See tags (view in online documentation for working links). + /// + /// Note: This only has an effect if is set to Mode.WalkableSurfaceWithSeam or Mode.WalkableSurfaceWithTag. + /// + /// Note: Only non-negative values are valid. + /// + [UnityEngine.Serialization.FormerlySerializedAs("area")] + public int surfaceID = 1; + + /// + /// Surface rasterization mode. + /// See: + /// + public Mode mode = Mode.WalkableSurface; + + AABBTree.Key treeKey; + + void OnEnable () { + // Clamp area, upper limit isn't really a hard limit, but if it gets much higher it will start to interfere with other stuff + surfaceID = Mathf.Clamp(surfaceID, 0, 1 << 25); + if (!treeKey.isValid) { + treeKey = tree.Add(CalculateBounds(), this); + if (this.dynamic) BatchedEvents.Add(this, BatchedEvents.Event.Custom, OnUpdate); + } + } + + void OnDisable () { + BatchedEvents.Remove(this); + var originalBounds = tree.Remove(treeKey); + treeKey = default; + if (!this.dynamic) { + var newBounds = CalculateBounds(); + // When using static baching, the bounds of the object may shrink. + // In particular, if the object has been rotated, the renderer's bounds will originally use an approximation of the AABB (presumably just the original AABB, but rotated and then axis aligned again), + // but after static batching, it actually looks at the new mesh (with the rotation baked in), and can generate a more precise AABB (which may be smaller). + // Therefore we say that it's ok as long as the original bounds contain the new bounds. + // This is fine, because the tree only needs a bounding box which contains the object. If it's too big, it will just be a bit more conservative. + // Also expand the original bounding box by a tiny amount to work around floating point errors. + originalBounds.Expand(0.001f); + newBounds.Encapsulate(originalBounds); + if ((newBounds.center - originalBounds.center).sqrMagnitude > 0.01f*0.01f || (newBounds.extents - originalBounds.extents).sqrMagnitude > 0.01f*0.01f) { + Debug.LogError("The RecastMeshObj has been moved or resized since it was enabled. You should set dynamic to true for moving objects, or disable the component while moving it. The bounds changed from " + originalBounds + " to " + newBounds, this); + } + } + } + + static void OnUpdate (RecastMeshObj[] components, int _) { + for (int i = 0; i < components.Length; i++) { + var comp = components[i]; + if (comp != null && comp.transform.hasChanged) { + var bounds = comp.CalculateBounds(); + if (tree.GetBounds(comp.treeKey) != bounds) tree.Move(comp.treeKey, bounds); + comp.transform.hasChanged = false; + } + } + } + + /// Fills the buffer with all RecastMeshObjs which intersect the specified bounds + public static void GetAllInBounds (List buffer, Bounds bounds) { + // Refreshes the tree if necessary + BatchedEvents.ProcessEvent(BatchedEvents.Event.Custom); + + if (!Application.isPlaying) { + var objs = UnityCompatibility.FindObjectsByTypeSorted(); + for (int i = 0; i < objs.Length; i++) { + if (objs[i].enabled) { + if (bounds.Intersects(objs[i].CalculateBounds())) { + buffer.Add(objs[i]); + } + } + } + return; + } else if (Time.timeSinceLevelLoad == 0) { + // Is is not guaranteed that all RecastMeshObj OnEnable functions have been called, so if it is the first frame since loading a new level + // try to initialize all RecastMeshObj objects. + var objs = UnityCompatibility.FindObjectsByTypeUnsorted(); + for (int i = 0; i < objs.Length; i++) objs[i].OnEnable(); + } + + tree.Query(bounds, buffer); + } + + /// + /// Resolves the geometry source that is to be used. + /// Will output either a MeshFilter, a Collider, or a 2D collider, never more than one. + /// If all are null, then no geometry could be found. + /// + /// See: + /// + public void ResolveMeshSource (out MeshFilter meshFilter, out Collider collider, out Collider2D collider2D) { + meshFilter = null; + collider = null; + collider2D = null; + switch (geometrySource) { + case GeometrySource.Auto: + if (TryGetComponent(out _) && TryGetComponent(out meshFilter) && meshFilter.sharedMesh != null) return; + if (TryGetComponent(out collider)) return; + TryGetComponent(out collider2D); + break; + case GeometrySource.MeshFilter: + TryGetComponent(out meshFilter); + break; + case GeometrySource.Collider: + if (TryGetComponent(out collider)) return; + TryGetComponent(out collider2D); + break; + default: + throw new System.ArgumentOutOfRangeException(); + } + } + + /// Calculates and returns the bounding box containing all geometry to be rasterized + private Bounds CalculateBounds () { + ResolveMeshSource(out var filter, out var coll, out var coll2D); + + if (coll != null) { + return coll.bounds; + } else if (coll2D != null) { + return coll2D.bounds; + } else if (filter != null) { + if (TryGetComponent(out var rend)) { + return rend.bounds; + } else { + Debug.LogError("Cannot use a MeshFilter as a geomtry source without a MeshRenderer attached to the same GameObject.", this); + return new Bounds(Vector3.zero, Vector3.one); + } + } else { + Debug.LogError("Could not find an attached mesh source", this); + return new Bounds(Vector3.zero, Vector3.one); + } + } + + protected override void OnUpgradeSerializedData (ref Serialization.Migrations migrations, bool unityThread) { + if (migrations.TryMigrateFromLegacyFormat(out var legacyVersion)) { + #pragma warning disable 618 + if (legacyVersion == 1) area = surfaceID; + #pragma warning restore 618 + if (legacyVersion <= 2) includeInScan = ScanInclusion.AlwaysInclude; + // Mode.ExcludeFromGraph was changed to ScanInclusion.AlwaysExclude + if (mode == (Mode)0) includeInScan = ScanInclusion.AlwaysExclude; + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs.meta new file mode 100644 index 0000000..5ef9823 --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/RecastMeshObj.cs.meta @@ -0,0 +1,7 @@ +fileFormatVersion: 2 +guid: 220345cbaa167417bbe806f230f68615 +MonoImporter: + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs new file mode 100644 index 0000000..03565bd --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs @@ -0,0 +1,341 @@ +namespace Pathfinding.Jobs { + using UnityEngine; + using Unity.Burst; + using Unity.Collections; + using Unity.Jobs; + using Unity.Mathematics; + using UnityEngine.Assertions; + using Pathfinding.Graphs.Grid; + using Pathfinding.Util; + + /// + /// Slice of a 3D array. + /// + /// This is a helper struct used in many jobs to make them work on a part of the data. + /// + /// The outer array has the size .x * .y * .z, laid out as if the coordinates were sorted by the tuple (Y,Z,X). + /// The inner array has the size .x * .y * .z, also laid out as if the coordinates were sorted by the tuple (Y,Z,X). + /// + public readonly struct Slice3D { + public readonly int3 outerSize; + public readonly IntBounds slice; + + public Slice3D (IntBounds outer, IntBounds slice) : this(outer.size, slice.Offset(-outer.min)) {} + + + public Slice3D (int3 outerSize, IntBounds slice) { + this.outerSize = outerSize; + this.slice = slice; + Assert.IsTrue(slice.min.x >= 0 && slice.min.y >= 0 && slice.min.z >= 0); + Assert.IsTrue(slice.max.x <= outerSize.x && slice.max.y <= outerSize.y && slice.max.z <= outerSize.z); + Assert.IsTrue(slice.size.x > 0 && slice.size.y > 0 && slice.size.z > 0); + } + + public void AssertMatchesOuter(UnsafeSpan values) where T : unmanaged { + Assert.AreEqual(outerSize.x * outerSize.y * outerSize.z, values.Length); + } + + public void AssertMatchesOuter(NativeArray values) where T : struct { + Assert.AreEqual(outerSize.x * outerSize.y * outerSize.z, values.Length); + } + + public void AssertMatchesInner(NativeArray values) where T : struct { + Assert.AreEqual(slice.size.x * slice.size.y * slice.size.z, values.Length); + } + + public void AssertSameSize (Slice3D other) { + Assert.AreEqual(slice.size, other.slice.size); + } + + public int InnerCoordinateToOuterIndex (int x, int y, int z) { + var(dx, dy, dz) = outerStrides; + return (x + slice.min.x) * dx + (y + slice.min.y) * dy + (z + slice.min.z) * dz; + } + + public int length => slice.size.x * slice.size.y * slice.size.z; + + public (int, int, int)outerStrides => (1, outerSize.x * outerSize.z, outerSize.x); + public (int, int, int)innerStrides => (1, slice.size.x * slice.size.z, slice.size.x); + public int outerStartIndex { + get { + var(dx, dy, dz) = outerStrides; + return slice.min.x * dx + slice.min.y * dy + slice.min.z * dz; + } + } + + /// True if the slice covers the whole outer array + public bool coversEverything => math.all(slice.size == outerSize); + } + + /// Helpers for scheduling simple NativeArray jobs + static class NativeArrayExtensions { + /// this[i] = value + public static JobMemSet MemSet(this NativeArray self, T value) where T : unmanaged { + return new JobMemSet { + data = self, + value = value, + }; + } + + /// this[i] &= other[i] + public static JobAND BitwiseAndWith (this NativeArray self, NativeArray other) { + return new JobAND { + result = self, + data = other, + }; + } + + /// to[i] = from[i] + public static JobCopy CopyToJob(this NativeArray from, NativeArray to) where T : struct { + return new JobCopy { + from = from, + to = to, + }; + } + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public static SliceActionJob WithSlice(this T action, Slice3D slice) where T : struct, GridIterationUtilities.ISliceAction { + return new SliceActionJob { + action = action, + slice = slice, + }; + } + + [System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)] + public static IndexActionJob WithLength(this T action, int length) where T : struct, GridIterationUtilities.ISliceAction { + return new IndexActionJob { + action = action, + length = length, + }; + } + + public static JobRotate3DArray Rotate3D(this NativeArray arr, int3 size, int dx, int dz) where T : unmanaged { + return new JobRotate3DArray { + arr = arr, + size = size, + dx = dx, + dz = dz, + }; + } + } + + /// + /// Treats input as a 3-dimensional array and copies it into the output at the specified position. + /// + /// The is a 3D array, and refers to a rectangular slice of this array. + /// The is defined similarly. + /// + /// The two slices must naturally have the same shape. + /// + [BurstCompile] + public struct JobCopyRectangle : IJob where T : struct { + [ReadOnly] + [DisableUninitializedReadCheck] // TODO: Fix so that job doesn't run instead + public NativeArray input; + + [WriteOnly] + public NativeArray output; + + public Slice3D inputSlice; + public Slice3D outputSlice; + + public void Execute () { + Copy(input, output, inputSlice, outputSlice); + } + + /// + /// Treats input as a 3-dimensional array and copies it into the output at the specified position. + /// + /// The input is a 3D array, and inputSlice refers to a rectangular slice of this array. + /// The output is defined similarly. + /// + /// The two slices must naturally have the same shape. + /// + public static void Copy (NativeArray input, NativeArray output, Slice3D inputSlice, Slice3D outputSlice) { + inputSlice.AssertMatchesOuter(input); + outputSlice.AssertMatchesOuter(output); + inputSlice.AssertSameSize(outputSlice); + + if (inputSlice.coversEverything && outputSlice.coversEverything) { + // One contiguous chunk + // TODO: Check can be made better by only checking if it is a contiguous chunk instead of covering the whole arrays + input.CopyTo(output); + } else { + // Copy row-by-row + for (int y = 0; y < outputSlice.slice.size.y; y++) { + for (int z = 0; z < outputSlice.slice.size.z; z++) { + var rowOffsetInput = inputSlice.InnerCoordinateToOuterIndex(0, y, z); + var rowOffsetOutput = outputSlice.InnerCoordinateToOuterIndex(0, y, z); + // Using a raw MemCpy call is a bit faster, but that requires unsafe code + // Using a for loop is *a lot* slower (except for very small arrays, in which case it is about the same or very slightly faster). + NativeArray.Copy(input, rowOffsetInput, output, rowOffsetOutput, outputSlice.slice.size.x); + } + } + } + } + } + + /// result[i] = value + [BurstCompile] + public struct JobMemSet : IJob where T : unmanaged { + [WriteOnly] + public NativeArray data; + + public T value; + + public void Execute() => data.AsUnsafeSpan().Fill(value); + } + + /// to[i] = from[i] + [BurstCompile] + public struct JobCopy : IJob where T : struct { + [ReadOnly] + public NativeArray from; + + [WriteOnly] + public NativeArray to; + + public void Execute () { + from.CopyTo(to); + } + } + + [BurstCompile] + public struct IndexActionJob : IJob where T : struct, GridIterationUtilities.ISliceAction { + public T action; + public int length; + + public void Execute () { + for (int i = 0; i < length; i++) action.Execute((uint)i, (uint)i); + } + } + + [BurstCompile] + public struct SliceActionJob : IJob where T : struct, GridIterationUtilities.ISliceAction { + public T action; + public Slice3D slice; + + public void Execute () { + GridIterationUtilities.ForEachCellIn3DSlice(slice, ref action); + } + } + + /// result[i] &= data[i] + public struct JobAND : GridIterationUtilities.ISliceAction { + public NativeArray result; + + [ReadOnly] + public NativeArray data; + + public void Execute (uint outerIdx, uint innerIdx) { + result[(int)outerIdx] &= data[(int)outerIdx]; + } + } + + [BurstCompile] + public struct JobMaxHitCount : IJob { + [ReadOnly] + public NativeArray hits; + public int maxHits; + public int layerStride; + [WriteOnly] + public NativeArray maxHitCount; + public void Execute () { + int maxHit = 0; + + for (; maxHit < maxHits; maxHit++) { + int offset = maxHit * layerStride; + bool any = false; + for (int i = offset; i < offset + layerStride; i++) { + if (math.any(hits[i].normal)) { + any = true; + break; + } + } + + if (!any) break; + } + + maxHitCount[0] = math.max(1, maxHit); + } + } + + /// + /// Copies hit points and normals. + /// points[i] = hits[i].point (if anything was hit), normals[i] = hits[i].normal.normalized. + /// + [BurstCompile(FloatMode = FloatMode.Fast)] + public struct JobCopyHits : IJob, GridIterationUtilities.ISliceAction { + [ReadOnly] + public NativeArray hits; + + [WriteOnly] + public NativeArray points; + + [WriteOnly] + public NativeArray normals; + public Slice3D slice; + + public void Execute () { + // The number of hits may be larger than the number of points. The remaining hits are not actually hits. + Assert.IsTrue(hits.Length >= slice.length); + slice.AssertMatchesOuter(points); + slice.AssertMatchesOuter(normals); + GridIterationUtilities.ForEachCellIn3DSlice(slice, ref this); + } + + public void Execute (uint outerIdx, uint innerIdx) { + Unity.Burst.CompilerServices.Aliasing.ExpectNotAliased(points, normals); + var normal = hits[(int)innerIdx].normal; + var normalV4 = new float4(normal.x, normal.y, normal.z, 0); + normals[(int)outerIdx] = math.normalizesafe(normalV4); + + // Check if anything was hit. The normal will be zero otherwise + // If nothing was hit then the existing data in the points array is reused + if (math.lengthsq(normalV4) > math.FLT_MIN_NORMAL) { + points[(int)outerIdx] = hits[(int)innerIdx].point; + } + } + } + + [BurstCompile] + public struct JobRotate3DArray: IJob where T : unmanaged { + public NativeArray arr; + public int3 size; + public int dx, dz; + + public void Execute () { + int width = size.x; + int height = size.y; + int depth = size.z; + var span = arr.AsUnsafeSpan(); + dx = dx % width; + dz = dz % depth; + if (dx != 0) { + if (dx < 0) dx = width + dx; + var tmp = new NativeArray(dx, Allocator.Temp); + var tmpSpan = tmp.AsUnsafeSpan(); + for (int y = 0; y < height; y++) { + var offset = y * width * depth; + for (int z = 0; z < depth; z++) { + span.Slice(offset + z * width + width - dx, dx).CopyTo(tmpSpan); + span.Move(offset + z * width, offset + z * width + dx, width - dx); + tmpSpan.CopyTo(span.Slice(offset + z * width, dx)); + } + } + } + + if (dz != 0) { + if (dz < 0) dz = depth + dz; + var tmp = new NativeArray(dz * width, Allocator.Temp); + var tmpSpan = tmp.AsUnsafeSpan(); + for (int y = 0; y < height; y++) { + var offset = y * width * depth; + span.Slice(offset + (depth - dz) * width, dz * width).CopyTo(tmpSpan); + span.Move(offset, offset + dz * width, (depth - dz) * width); + tmpSpan.CopyTo(span.Slice(offset, dz * width)); + } + } + } + } +} diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs.meta new file mode 100644 index 0000000..aa515bf --- /dev/null +++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Graphs/Utilities/UtilityJobs.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c3924c162ff1c47bf914318910ba0a43 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: -- cgit v1.1-26-g67d0