+ astar project

1 files changed, 372 insertions, 0 deletions

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<GraphNode> del);
+	}
+
+	/// <summary>
+	/// 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 <see cref="FunnelModifier"/> 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 <see cref="RecastGraph"/>.
+	///
+	/// 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
+	/// </summary>
+	[JsonOptIn]
+	[Pathfinding.Util.Preserve]
+	public class NavMeshGraph : NavmeshBase, IUpdatableGraph {
+		/// <summary>Mesh to construct navmesh from</summary>
+		[JsonMember]
+		public Mesh sourceMesh;
+
+		/// <summary>Offset in world space</summary>
+		[JsonMember]
+		public Vector3 offset;
+
+		/// <summary>Rotation in degrees</summary>
+		[JsonMember]
+		public Vector3 rotation;
+
+		/// <summary>Scale of the graph</summary>
+		[JsonMember]
+		public float scale = 1;
+
+		/// <summary>
+		/// 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 <see cref="PointOnNavmesh"/> 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.
+		/// </summary>
+		[JsonMember]
+		public bool recalculateNormals = true;
+
+		/// <summary>
+		/// Cached bounding box minimum of <see cref="sourceMesh"/>.
+		/// 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 <see cref="CalculateTransform"/> method uses it.
+		/// </summary>
+		[JsonMember]
+		Vector3 cachedSourceMeshBoundsMin;
+
+		/// <summary>
+		/// Radius to use when expanding navmesh cuts.
+		///
+		/// See: <see cref="NavmeshCut.radiusExpansionMode"/>
+		/// </summary>
+		[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;
+
+		/// <summary>
+		/// 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.
+		/// </summary>
+		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;
+		}
+
+		/// <summary>
+		/// 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]
+		/// </summary>
+		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<GraphUpdateObject> 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<GraphUpdateObject> 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<JobHandle> 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<JobCalculateTriangleConnections.TileNodeConnectionsUnsafe>(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);
+		}
+	}
+}