StarEscort/Unity/Assets/Plugins/AstarPathfindingProject/Utilities/Funnel.cs

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace Pathfinding {
	using Pathfinding.Util;

	/// <summary>
	/// Implements the funnel algorithm as well as various related methods.
	/// See: http://digestingduck.blogspot.se/2010/03/simple-stupid-funnel-algorithm.html
	/// See: FunnelModifier for the component that you can attach to objects to use the funnel algorithm.
	/// </summary>
	public class Funnel {
		/// <summary>Funnel in which the path to the target will be</summary>
		public struct FunnelPortals {
			public List<Vector3> left;
			public List<Vector3> right;
		}

		/// <summary>
		/// Part of a path.
		/// This is either a sequence of adjacent triangles
		/// or a link.
		/// See: NodeLink2
		/// </summary>
		public struct PathPart {
			/// <summary>Index of the first node in this part</summary>
			public int startIndex;
			/// <summary>Index of the last node in this part</summary>
			public int endIndex;
			public Vector3 startPoint, endPoint;
			public bool isLink;
		}

		public static List<PathPart> SplitIntoParts (Path path) {
			var nodes = path.path;

			var result = ListPool<PathPart>.Claim();

			if (nodes == null || nodes.Count == 0) {
				return result;
			}

			// Loop through the path and split it into
			// parts joined by links
			for (int i = 0; i < nodes.Count; i++) {
				if (nodes[i] is TriangleMeshNode || nodes[i] is GridNodeBase) {
					var part = new PathPart();
					part.startIndex = i;
					uint currentGraphIndex = nodes[i].GraphIndex;

					// Loop up until we find a node in another graph
					// Ignore NodeLink3 nodes
					for (; i < nodes.Count; i++) {
						if (nodes[i].GraphIndex != currentGraphIndex && !(nodes[i] is NodeLink3Node)) {
							break;
						}
					}

					i--;
					part.endIndex = i;

					// If this is the first part in the path, use the exact start point
					// otherwise use the position of the node right before the start of this
					// part which is likely the end of the link to this part
					if (part.startIndex == 0) {
						part.startPoint = path.vectorPath[0];
					} else {
						part.startPoint = (Vector3)nodes[part.startIndex-1].position;
					}

					if (part.endIndex == nodes.Count-1) {
						part.endPoint = path.vectorPath[path.vectorPath.Count-1];
					} else {
						part.endPoint = (Vector3)nodes[part.endIndex+1].position;
					}

					result.Add(part);
				} else if (NodeLink2.GetNodeLink(nodes[i]) != null) {
					var part = new PathPart();
					part.startIndex = i;
					var currentGraphIndex = nodes[i].GraphIndex;

					for (i++; i < nodes.Count; i++) {
						if (nodes[i].GraphIndex != currentGraphIndex) {
							break;
						}
					}
					i--;

					if (i - part.startIndex == 0) {
						// Just ignore it, it might be the case that a NodeLink was the closest node
						continue;
					} else if (i - part.startIndex != 1) {
						throw new System.Exception("NodeLink2 link length greater than two (2) nodes. " + (i - part.startIndex + 1));
					}

					part.endIndex = i;
					part.isLink = true;
					part.startPoint = (Vector3)nodes[part.startIndex].position;
					part.endPoint = (Vector3)nodes[part.endIndex].position;
					result.Add(part);
				} else {
					throw new System.Exception("Unsupported node type or null node");
				}
			}

			return result;
		}

		public static FunnelPortals ConstructFunnelPortals (List<GraphNode> nodes, PathPart part) {
			if (nodes == null || nodes.Count == 0) {
				return new FunnelPortals { left = ListPool<Vector3>.Claim(0), right = ListPool<Vector3>.Claim(0) };
			}

			if (part.endIndex < part.startIndex || part.startIndex < 0 || part.endIndex > nodes.Count) throw new System.ArgumentOutOfRangeException();

			// Claim temporary lists and try to find lists with a high capacity
			var left = ListPool<Vector3>.Claim(nodes.Count+1);
			var right = ListPool<Vector3>.Claim(nodes.Count+1);

			// Add start point
			left.Add(part.startPoint);
			right.Add(part.startPoint);

			// Loop through all nodes in the path (except the last one)
			for (int i = part.startIndex; i < part.endIndex; i++) {
				// Get the portal between path[i] and path[i+1] and add it to the left and right lists
				bool portalWasAdded = nodes[i].GetPortal(nodes[i+1], left, right, false);

				if (!portalWasAdded) {
					// Fallback, just use the positions of the nodes
					left.Add((Vector3)nodes[i].position);
					right.Add((Vector3)nodes[i].position);

					left.Add((Vector3)nodes[i+1].position);
					right.Add((Vector3)nodes[i+1].position);
				}
			}

			// Add end point
			left.Add(part.endPoint);
			right.Add(part.endPoint);

			return new FunnelPortals { left = left, right = right };
		}

		public static void ShrinkPortals (FunnelPortals portals, float shrink) {
			if (shrink <= 0.00001f) return;

			for (int i = 0; i < portals.left.Count; i++) {
				var left = portals.left[i];
				var right = portals.right[i];

				var length = (left - right).magnitude;
				if (length > 0) {
					float s = Mathf.Min(shrink / length, 0.4f);
					portals.left[i] = Vector3.Lerp(left, right, s);
					portals.right[i] = Vector3.Lerp(left, right, 1 - s);
				}
			}
		}

		static bool UnwrapHelper (Vector3 portalStart, Vector3 portalEnd, Vector3 prevPoint, Vector3 nextPoint, ref Quaternion mRot, ref Vector3 mOffset) {
			// Skip the point if it was on the rotation axis
			if (VectorMath.IsColinear(portalStart, portalEnd, nextPoint)) {
				return false;
			}

			var axis = portalEnd - portalStart;
			var sqrMagn = axis.sqrMagnitude;
			prevPoint -= Vector3.Dot(prevPoint - portalStart, axis)/sqrMagn * axis;
			nextPoint -= Vector3.Dot(nextPoint - portalStart, axis)/sqrMagn * axis;
			var rot = Quaternion.FromToRotation(nextPoint - portalStart, portalStart - prevPoint);

			// The code below is equivalent to these matrix operations (but a lot faster)
			// This represents a rotation around a line in 3D space
			//mat = mat * Matrix4x4.TRS(portalStart, rot, Vector3.one) * Matrix4x4.TRS(-portalStart, Quaternion.identity, Vector3.one);
			mOffset += mRot * (portalStart - rot * portalStart);
			mRot *= rot;

			return true;
		}

		/// <summary>
		/// Unwraps the funnel portals from 3D space to 2D space.
		/// The result is stored in the left and right arrays which must be at least as large as the funnel.left and funnel.right lists.
		///
		/// The input is a funnel like in the image below. It may be rotated and twisted.
		/// [Open online documentation to see images]
		/// The output will be a funnel in 2D space like in the image below. All twists and bends will have been straightened out.
		/// [Open online documentation to see images]
		///
		/// See: <see cref="Calculate(FunnelPortals,bool,bool)"/>
		/// </summary>
		public static void Unwrap (FunnelPortals funnel, Vector2[] left, Vector2[] right) {
			int startingIndex = 1;
			var normal = Vector3.Cross(funnel.right[1] - funnel.left[0], funnel.left[1] - funnel.left[0]);

			// This handles the case when the starting point is colinear with the first portal.
			// Note that left.Length is only guaranteed to be at least as large as funnel.left.Count, it may be larger.
			while (normal.sqrMagnitude <= 0.00000001f && startingIndex + 1 < funnel.left.Count) {
				startingIndex++;
				normal = Vector3.Cross(funnel.right[startingIndex] - funnel.left[0], funnel.left[startingIndex] - funnel.left[0]);
			}

			left[0] = right[0] = Vector2.zero;

			var portalLeft = funnel.left[1];
			var portalRight = funnel.right[1];
			var prevPoint = funnel.left[0];

			// The code below is equivalent to this matrix (but a lot faster)
			// This represents a rotation around a line in 3D space
			// Matrix4x4 m = Matrix4x4.TRS(Vector3.zero, Quaternion.FromToRotation(normal, Vector3.forward), Vector3.one) * Matrix4x4.TRS(-funnel.right[0], Quaternion.identity, Vector3.one);
			Quaternion mRot = Quaternion.FromToRotation(normal, Vector3.forward);
			Vector3 mOffset = mRot * (-funnel.right[0]);

			for (int i = 1; i < funnel.left.Count; i++) {
				if (UnwrapHelper(portalLeft, portalRight, prevPoint, funnel.left[i], ref mRot, ref mOffset)) {
					prevPoint = portalLeft;
					portalLeft = funnel.left[i];
				}

				left[i] = mRot * funnel.left[i] + mOffset;

				if (UnwrapHelper(portalLeft, portalRight, prevPoint, funnel.right[i], ref mRot, ref mOffset)) {
					prevPoint = portalRight;
					portalRight = funnel.right[i];
				}

				right[i] = mRot * funnel.right[i] + mOffset;
			}
		}

		/// <summary>
		/// Try to fix degenerate or invalid funnels.
		/// Returns: The number of vertices at the start of both arrays that should be ignored or -1 if the algorithm failed.
		/// </summary>
		static int FixFunnel (Vector2[] left, Vector2[] right, int numPortals) {
			if (numPortals > left.Length || numPortals > right.Length) throw new System.ArgumentException("Arrays do not have as many elements as specified");

			if (numPortals < 3) {
				return -1;
			}

			// Remove duplicate vertices
			int startIndex = 0;
			while (left[startIndex + 1] == left[startIndex + 2] && right[startIndex + 1] == right[startIndex + 2]) {
				// Equivalent to RemoveAt(1) if they would have been lists
				left[startIndex + 1] = left[startIndex + 0];
				right[startIndex + 1] = right[startIndex + 0];
				startIndex++;

				if (numPortals - startIndex < 3) {
					return -1;
				}
			}

			return startIndex;
		}

		protected static Vector2 ToXZ (Vector3 p) {
			return new Vector2(p.x, p.z);
		}

		protected static Vector3 FromXZ (Vector2 p) {
			return new Vector3(p.x, 0, p.y);
		}

		/// <summary>True if b is to the right of or on the line from (0,0) to a</summary>
		protected static bool RightOrColinear (Vector2 a, Vector2 b) {
			return (a.x*b.y - b.x*a.y) <= 0;
		}

		/// <summary>True if b is to the left of or on the line from (0,0) to a</summary>
		protected static bool LeftOrColinear (Vector2 a, Vector2 b) {
			return (a.x*b.y - b.x*a.y) >= 0;
		}

		/// <summary>
		/// Calculate the shortest path through the funnel.
		///
		/// If the unwrap option is disabled the funnel will simply be projected onto the XZ plane.
		/// If the unwrap option is enabled then the funnel may be oriented arbitrarily and may have twists and bends.
		/// This makes it possible to support the funnel algorithm in XY space as well as in more complicated cases, such
		/// as on curved worlds.
		/// [Open online documentation to see images]
		///
		/// [Open online documentation to see images]
		///
		/// See: Unwrap
		/// </summary>
		/// <param name="funnel">The portals of the funnel. The first and last vertices portals must be single points (so for example left[0] == right[0]).</param>
		/// <param name="unwrap">Determines if twists and bends should be straightened out before running the funnel algorithm.</param>
		/// <param name="splitAtEveryPortal">If true, then a vertex will be inserted every time the path crosses a portal
		///  instead of only at the corners of the path. The result will have exactly one vertex per portal if this is enabled.
		///  This may introduce vertices with the same position in the output (esp. in corners where many portals meet).</param>
		public static List<Vector3> Calculate (FunnelPortals funnel, bool unwrap, bool splitAtEveryPortal) {
			if (funnel.left.Count != funnel.right.Count) throw new System.ArgumentException("funnel.left.Count != funnel.right.Count");

			// Get arrays at least as large as the number of portals
			var leftArr = ArrayPool<Vector2>.Claim(funnel.left.Count);
			var rightArr = ArrayPool<Vector2>.Claim(funnel.left.Count);

			if (unwrap) {
				Unwrap(funnel, leftArr, rightArr);
			} else {
				// Copy to arrays
				for (int i = 0; i < funnel.left.Count; i++) {
					leftArr[i] = ToXZ(funnel.left[i]);
					rightArr[i] = ToXZ(funnel.right[i]);
				}
			}

			int startIndex = FixFunnel(leftArr, rightArr, funnel.left.Count);
			var intermediateResult = ListPool<int>.Claim();
			if (startIndex == -1) {
				// If funnel algorithm failed, fall back to a simple line
				intermediateResult.Add(0);
				intermediateResult.Add(funnel.left.Count - 1);
			} else {
				bool lastCorner;
				Calculate(leftArr, rightArr, funnel.left.Count, startIndex, intermediateResult, int.MaxValue, out lastCorner);
			}

			// Get list for the final result
			var result = ListPool<Vector3>.Claim(intermediateResult.Count);

			Vector2 prev2D = leftArr[0];
			var prevIdx = 0;
			for (int i = 0; i < intermediateResult.Count; i++) {
				var idx = intermediateResult[i];

				if (splitAtEveryPortal) {
					// Check intersections with every portal segment
					var next2D = idx >= 0 ? leftArr[idx] : rightArr[-idx];
					for (int j = prevIdx + 1; j < System.Math.Abs(idx); j++) {
						// var factor = VectorMath.LineIntersectionFactorXZ(FromXZ(leftArr[j]), FromXZ(rightArr[j]), FromXZ(prev2D), FromXZ(next2D));
						if (!VectorMath.LineLineIntersectionFactor(leftArr[j], rightArr[j] - leftArr[j], prev2D, next2D - prev2D, out float factor)) {
							// This really shouldn't happen
							factor = 0.5f;
						}
						result.Add(Vector3.Lerp(funnel.left[j], funnel.right[j], factor));
					}

					prevIdx = Mathf.Abs(idx);
					prev2D = next2D;
				}

				if (idx >= 0) {
					result.Add(funnel.left[idx]);
				} else {
					result.Add(funnel.right[-idx]);
				}
			}

			// Release lists back to the pool
			ListPool<int>.Release(ref intermediateResult);
			ArrayPool<Vector2>.Release(ref leftArr);
			ArrayPool<Vector2>.Release(ref rightArr);
			return result;
		}

		/// <summary>
		/// Funnel algorithm.
		/// funnelPath will be filled with the result.
		/// The result is the indices of the vertices that were picked, a non-negative value refers to the corresponding index in the
		/// left array, a negative value refers to the corresponding index in the right array.
		/// So e.g 5 corresponds to left[5] and -2 corresponds to right[2]
		///
		/// See: http://digestingduck.blogspot.se/2010/03/simple-stupid-funnel-algorithm.html
		/// </summary>
		static void Calculate (Vector2[] left, Vector2[] right, int numPortals, int startIndex, List<int> funnelPath, int maxCorners, out bool lastCorner) {
			if (left.Length != right.Length) throw new System.ArgumentException();

			lastCorner = false;

			int apexIndex = startIndex + 0;
			int rightIndex = startIndex + 1;
			int leftIndex = startIndex + 1;

			Vector2 portalApex = left[apexIndex];
			Vector2 portalLeft = left[leftIndex];
			Vector2 portalRight = right[rightIndex];

			funnelPath.Add(apexIndex);

			for (int i = startIndex + 2; i < numPortals; i++) {
				if (funnelPath.Count >= maxCorners) {
					return;
				}

				if (funnelPath.Count > 2000) {
					Debug.LogWarning("Avoiding infinite loop. Remove this check if you have this long paths.");
					break;
				}

				Vector2 pLeft = left[i];
				Vector2 pRight = right[i];

				if (LeftOrColinear(portalRight - portalApex, pRight - portalApex)) {
					if (portalApex == portalRight || RightOrColinear(portalLeft - portalApex, pRight - portalApex)) {
						portalRight = pRight;
						rightIndex = i;
					} else {
						portalApex = portalRight = portalLeft;
						i = apexIndex = rightIndex = leftIndex;

						funnelPath.Add(apexIndex);
						continue;
					}
				}

				if (RightOrColinear(portalLeft - portalApex, pLeft - portalApex)) {
					if (portalApex == portalLeft || LeftOrColinear(portalRight - portalApex, pLeft - portalApex)) {
						portalLeft = pLeft;
						leftIndex = i;
					} else {
						portalApex = portalLeft = portalRight;
						i = apexIndex = leftIndex = rightIndex;

						// Negative value because we are referring
						// to the right side
						funnelPath.Add(-apexIndex);

						continue;
					}
				}
			}

			lastCorner = true;
			funnelPath.Add(numPortals-1);
		}
	}
}