+ astar project

1 files changed, 283 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/StartEndModifier.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/StartEndModifier.cs
new file mode 100644
index 0000000..0759e85
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Modifiers/StartEndModifier.cs
@@ -0,0 +1,283 @@
+using UnityEngine;
+using System.Collections.Generic;
+
+namespace Pathfinding {
+	[System.Serializable]
+	/// <summary>
+	/// Adjusts start and end points of a path.
+	///
+	/// This modifier is included in the <see cref="Pathfinding.Seeker"/> component and is always used if you are using a Seeker.
+	/// When a path is calculated the resulting path will only be the positions of the nodes it passes through.
+	/// However often you may not want to navigate to the center of a specific node but instead to a point on the surface of a node.
+	/// This modifier will adjust the endpoints of the path.
+	///
+	/// [Open online documentation to see images]
+	/// </summary>
+	public class StartEndModifier : PathModifier {
+		public override int Order { get { return 0; } }
+
+		/// <summary>
+		/// Add points to the path instead of replacing them.
+		/// If for example <see cref="exactEndPoint"/> is set to ClosestOnNode then the path will be modified so that
+		/// the path goes first to the center of the last node in the path and then goes to the closest point
+		/// on the node to the end point in the path request.
+		///
+		/// If this is false however then the relevant points in the path will simply be replaced.
+		/// In the above example the path would go directly to the closest point on the node without passing
+		/// through the center of the node.
+		/// </summary>
+		public bool addPoints;
+
+		/// <summary>
+		/// How the start point of the path will be determined.
+		/// See: <see cref="Exactness"/>
+		/// </summary>
+		public Exactness exactStartPoint = Exactness.ClosestOnNode;
+
+		/// <summary>
+		/// How the end point of the path will be determined.
+		/// See: <see cref="Exactness"/>
+		/// </summary>
+		public Exactness exactEndPoint = Exactness.ClosestOnNode;
+
+		/// <summary>
+		/// Will be called when a path is processed.
+		/// The value which is returned will be used as the start point of the path
+		/// and potentially clamped depending on the value of the <see cref="exactStartPoint"/> field.
+		/// Only used for the Original, Interpolate and NodeConnection modes.
+		/// </summary>
+		public System.Func<Vector3> adjustStartPoint;
+
+		/// <summary>
+		/// Sets where the start and end points of a path should be placed.
+		///
+		/// Here is a legend showing what the different items in the above images represent.
+		/// The images above show a path coming in from the top left corner and ending at a node next to an obstacle as well as 2 different possible end points of the path and how they would be modified.
+		/// [Open online documentation to see images]
+		/// </summary>
+		public enum Exactness {
+			/// <summary>
+			/// The point is snapped to the position of the first/last node in the path.
+			/// Use this if your game is very tile based and you want your agents to stop precisely at the center of the nodes.
+			/// If you recalculate the path while the agent is moving you may want the start point snapping to be ClosestOnNode and the end point snapping to be SnapToNode however
+			/// as while an agent is moving it will likely not be right at the center of a node.
+			///
+			/// [Open online documentation to see images]
+			/// </summary>
+			SnapToNode,
+			/// <summary>
+			/// The point is set to the exact point which was passed when creating the path request.
+			/// Note that if a path was for example requested to a point inside an obstacle, then the last point of the path will be inside that obstacle, which is usually not what you want.
+			/// Consider using the <see cref="Exactness.ClosestOnNode"/> option instead.
+			///
+			/// [Open online documentation to see images]
+			/// </summary>
+			Original,
+			/// <summary>
+			/// The point is set to the closest point on the line between either the two first points or the two last points.
+			/// Usually you will want to use the NodeConnection mode instead since that is usually the behaviour that you really want.
+			/// This mode exists mostly for compatibility reasons.
+			/// [Open online documentation to see images]
+			/// Deprecated: Use NodeConnection instead.
+			/// </summary>
+			Interpolate,
+			/// <summary>
+			/// The point is set to the closest point on the surface of the node. Note that some node types (point nodes) do not have a surface, so the "closest point" is simply the node's position which makes this identical to <see cref="Exactness.SnapToNode"/>.
+			/// This is the mode that you almost always want to use in a free movement 3D world.
+			/// [Open online documentation to see images]
+			/// </summary>
+			ClosestOnNode,
+			/// <summary>
+			/// The point is set to the closest point on one of the connections from the start/end node.
+			/// This mode may be useful in a grid based or point graph based world when using the AILerp script.
+			///
+			/// Note: If you are using this mode with a <see cref="Pathfinding.PointGraph"/> you probably also want to use the <see cref="Pathfinding.PointGraph.NodeDistanceMode Connection"/> for <see cref="PointGraph.nearestNodeDistanceMode"/>.
+			///
+			/// [Open online documentation to see images]
+			/// </summary>
+			NodeConnection,
+		}
+
+		/// <summary>
+		/// Do a straight line check from the node's center to the point determined by the <see cref="Exactness"/>.
+		/// There are very few cases where you will want to use this. It is mostly here for
+		/// backwards compatibility reasons.
+		///
+		/// Version: Since 4.1 this field only has an effect for the <see cref="Exactness"/> mode Original because that's the only one where it makes sense.
+		/// </summary>
+		public bool useRaycasting;
+		public LayerMask mask = -1;
+
+		/// <summary>
+		/// Do a straight line check from the node's center to the point determined by the <see cref="Exactness"/>.
+		/// See: <see cref="useRaycasting"/>
+		///
+		/// Version: Since 4.1 this field only has an effect for the <see cref="Exactness"/> mode Original because that's the only one where it makes sense.
+		/// </summary>
+		public bool useGraphRaycasting;
+
+		List<GraphNode> connectionBuffer;
+		System.Action<GraphNode> connectionBufferAddDelegate;
+
+		public override void Apply (Path _p) {
+			var p = _p as ABPath;
+
+			// This modifier only supports ABPaths (doesn't make much sense for other paths anyway)
+			if (p == null || p.vectorPath.Count == 0) return;
+
+			bool singleNode = false;
+
+			if (p.vectorPath.Count == 1 && !addPoints) {
+				// Duplicate first point
+				p.vectorPath.Add(p.vectorPath[0]);
+				singleNode = true;
+			}
+
+			// Add instead of replacing points
+			bool forceAddStartPoint, forceAddEndPoint;
+			// Which connection the start/end point was on (only used for the Connection mode)
+			int closestStartConnection, closestEndConnection;
+
+			Vector3 pStart = Snap(p, exactStartPoint, true, out forceAddStartPoint, out closestStartConnection);
+			Vector3 pEnd = Snap(p, exactEndPoint, false, out forceAddEndPoint, out closestEndConnection);
+
+			// This is a special case when the path is only a single node and the Connection mode is used.
+			// (forceAddStartPoint/forceAddEndPoint is only used for the Connection mode)
+			// In this case the start and end points lie on the connections of the node.
+			// There are two cases:
+			// 1. If the start and end points lie on the same connection we do *not* want
+			// the path to pass through the node center but instead go directly from point to point.
+			// This is the case of closestStartConnection == closestEndConnection.
+			// 2. If the start and end points lie on different connections we *want*
+			// the path to pass through the node center as it goes from one connection to another one.
+			// However in any case we only want the node center to be added once to the path
+			// so we set forceAddStartPoint to false anyway.
+			if (singleNode) {
+				if (closestStartConnection == closestEndConnection) {
+					forceAddStartPoint = false;
+					forceAddEndPoint = false;
+				} else {
+					forceAddStartPoint = false;
+				}
+			}
+
+			// Add or replace the start point
+			// Disable adding of points if the mode is SnapToNode since then
+			// the first item in vectorPath will very likely be the same as the
+			// position of the first node
+			if ((forceAddStartPoint || addPoints) && exactStartPoint != Exactness.SnapToNode) {
+				p.vectorPath.Insert(0, pStart);
+			} else {
+				p.vectorPath[0] = pStart;
+			}
+
+			if ((forceAddEndPoint || addPoints) && exactEndPoint != Exactness.SnapToNode) {
+				p.vectorPath.Add(pEnd);
+			} else {
+				p.vectorPath[p.vectorPath.Count-1] = pEnd;
+			}
+		}
+
+		Vector3 Snap (ABPath path, Exactness mode, bool start, out bool forceAddPoint, out int closestConnectionIndex) {
+			var index = start ? 0 : path.path.Count - 1;
+			var node = path.path[index];
+			var nodePos = (Vector3)node.position;
+
+			closestConnectionIndex = 0;
+
+			forceAddPoint = false;
+
+			switch (mode) {
+			case Exactness.ClosestOnNode:
+				return start ? path.startPoint : path.endPoint;
+			case Exactness.SnapToNode:
+				return nodePos;
+			case Exactness.Original:
+			case Exactness.Interpolate:
+			case Exactness.NodeConnection:
+				Vector3 relevantPoint;
+				if (start) {
+					relevantPoint = adjustStartPoint != null? adjustStartPoint() : path.originalStartPoint;
+				} else {
+					relevantPoint = path.originalEndPoint;
+				}
+
+				switch (mode) {
+				case Exactness.Original:
+					return GetClampedPoint(nodePos, relevantPoint, node);
+				case Exactness.Interpolate:
+					// Adjacent node to either the start node or the end node in the path
+					var adjacentNode = path.path[Mathf.Clamp(index + (start ? 1 : -1), 0, path.path.Count-1)];
+					return VectorMath.ClosestPointOnSegment(nodePos, (Vector3)adjacentNode.position, relevantPoint);
+				case Exactness.NodeConnection:
+					// This code uses some tricks to avoid allocations
+					// even though it uses delegates heavily
+					// The connectionBufferAddDelegate delegate simply adds whatever node
+					// it is called with to the connectionBuffer
+					connectionBuffer = connectionBuffer ?? new List<GraphNode>();
+					connectionBufferAddDelegate = connectionBufferAddDelegate ?? (System.Action<GraphNode>)connectionBuffer.Add;
+
+					// Adjacent node to either the start node or the end node in the path
+					adjacentNode = path.path[Mathf.Clamp(index + (start ? 1 : -1), 0, path.path.Count-1)];
+
+					// Add all neighbours of #node to the connectionBuffer
+					node.GetConnections(connectionBufferAddDelegate);
+					var bestPos = nodePos;
+					var bestDist = float.PositiveInfinity;
+
+					// Loop through all neighbours
+					// Do it in reverse order because the length of the connectionBuffer
+					// will change during iteration
+					for (int i = connectionBuffer.Count - 1; i >= 0; i--) {
+						var neighbour = connectionBuffer[i];
+						if (!path.CanTraverse(neighbour)) continue;
+
+						// Find the closest point on the connection between the nodes
+						// and check if the distance to that point is lower than the previous best
+						var closest = VectorMath.ClosestPointOnSegment(nodePos, (Vector3)neighbour.position, relevantPoint);
+
+						var dist = (closest - relevantPoint).sqrMagnitude;
+						if (dist < bestDist) {
+							bestPos = closest;
+							bestDist = dist;
+							closestConnectionIndex = i;
+
+							// If this node is not the adjacent node
+							// then the path should go through the start node as well
+							forceAddPoint = neighbour != adjacentNode;
+						}
+					}
+
+					connectionBuffer.Clear();
+					return bestPos;
+				default:
+					throw new System.ArgumentException("Cannot reach this point, but the compiler is not smart enough to realize that.");
+				}
+			default:
+				throw new System.ArgumentException("Invalid mode");
+			}
+		}
+
+		protected Vector3 GetClampedPoint (Vector3 from, Vector3 to, GraphNode hint) {
+			Vector3 point = to;
+			RaycastHit hit;
+
+			if (useRaycasting && Physics.Linecast(from, to, out hit, mask)) {
+				point = hit.point;
+			}
+
+			if (useGraphRaycasting && hint != null) {
+				var rayGraph = AstarData.GetGraph(hint) as IRaycastableGraph;
+
+				if (rayGraph != null) {
+					GraphHitInfo graphHit;
+					if (rayGraph.Linecast(from, point, out graphHit)) {
+						point = graphHit.point;
+					}
+				}
+			}
+
+			return point;
+		}
+	}
+}