ProjectDDD/Packages/com.arongranberg.astar/Modifiers/SimpleSmoothModifier.cs

using UnityEngine;
using System.Collections.Generic;
using Pathfinding.Pooling;

namespace Pathfinding {
	[AddComponentMenu("Pathfinding/Modifiers/Simple Smooth Modifier")]
	[System.Serializable]
	[RequireComponent(typeof(Seeker))]
	/// <summary>
	/// Modifier which smooths the path. This modifier can smooth a path by either moving the points closer together (Simple) or using Bezier curves (Bezier).
	///
	/// Attach this component to the same GameObject as a Seeker component.
	///
	/// This component will hook in to the Seeker's path post-processing system and will post process any paths it searches for.
	/// Take a look at the Modifier Priorities settings on the Seeker component to determine where in the process this modifier should process the path.
	///
	/// Several smoothing types are available, here follows a list of them and a short description of what they do, and how they work.
	/// But the best way is really to experiment with them yourself.
	///
	/// - <b>Simple</b> Smooths the path by drawing all points close to each other. This results in paths that might cut corners if you are not careful.
	/// It will also subdivide the path to create more more points to smooth as otherwise it would still be quite rough.
	/// [Open online documentation to see images]
	/// - <b>Bezier</b> Smooths the path using Bezier curves. This results a smooth path which will always pass through all points in the path, but make sure it doesn't turn too quickly.
	/// [Open online documentation to see images]
	/// - <b>OffsetSimple</b> An alternative to Simple smooth which will offset the path outwards in each step to minimize the corner-cutting.
	/// But be careful, if too high values are used, it will turn into loops and look really ugly.
	/// - <b>Curved Non Uniform</b> [Open online documentation to see images]
	///
	/// Note: Modifies vectorPath array
	/// TODO: Make the smooth modifier take the world geometry into account when smoothing
	/// </summary>
	[HelpURL("https://arongranberg.com/astar/documentation/stable/simplesmoothmodifier.html")]
	public class SimpleSmoothModifier : MonoModifier {
#if UNITY_EDITOR
		[UnityEditor.MenuItem("CONTEXT/Seeker/Add Simple Smooth Modifier")]
		public static void AddComp (UnityEditor.MenuCommand command) {
			(command.context as Component).gameObject.AddComponent(typeof(SimpleSmoothModifier));
		}
#endif

		public override int Order { get { return 50; } }

		/// <summary>Type of smoothing to use</summary>
		public SmoothType smoothType = SmoothType.Simple;

		/// <summary>Number of times to subdivide when not using a uniform length</summary>
		[Tooltip("The number of times to subdivide (divide in half) the path segments. [0...inf] (recommended [1...10])")]
		public int subdivisions = 2;

		/// <summary>Number of times to apply smoothing</summary>
		[Tooltip("Number of times to apply smoothing")]
		public int iterations = 2;

		/// <summary>Determines how much smoothing to apply in each smooth iteration. 0.5 usually produces the nicest looking curves.</summary>
		[Tooltip("Determines how much smoothing to apply in each smooth iteration. 0.5 usually produces the nicest looking curves")]
		[Range(0, 1)]
		public float strength = 0.5F;

		/// <summary>
		/// Toggle to divide all lines in equal length segments.
		/// See: <see cref="maxSegmentLength"/>
		/// </summary>
		[Tooltip("Toggle to divide all lines in equal length segments")]
		public bool uniformLength = true;

		/// <summary>
		/// The length of the segments in the smoothed path when using <see cref="uniformLength"/>.
		/// A high value yields rough paths and low value yields very smooth paths, but is slower
		/// </summary>
		[Tooltip("The length of each segment in the smoothed path. A high value yields rough paths and low value yields very smooth paths, but is slower")]
		public float maxSegmentLength = 2F;

		/// <summary>Length factor of the bezier curves' tangents'</summary>
		[Tooltip("Length factor of the bezier curves' tangents")]
		public float bezierTangentLength = 0.4F;

		/// <summary>Offset to apply in each smoothing iteration when using Offset Simple. See: <see cref="smoothType"/></summary>
		[Tooltip("Offset to apply in each smoothing iteration when using Offset Simple")]
		public float offset = 0.2F;

		/// <summary>Roundness factor used for CurvedNonuniform</summary>
		[Tooltip("How much to smooth the path. A higher value will give a smoother path, but might take the character far off the optimal path.")]
		public float factor = 0.1F;

		public enum SmoothType {
			Simple,
			Bezier,
			OffsetSimple,
			CurvedNonuniform
		}

		public override void Apply (Path p) {
			// This should never trigger unless some other modifier has messed stuff up
			if (p.vectorPath == null) {
				Debug.LogWarning("Can't process NULL path (has another modifier logged an error?)");
				return;
			}

			List<Vector3> path = null;

			switch (smoothType) {
			case SmoothType.Simple:
				path = SmoothSimple(p.vectorPath); break;
			case SmoothType.Bezier:
				path = SmoothBezier(p.vectorPath); break;
			case SmoothType.OffsetSimple:
				path = SmoothOffsetSimple(p.vectorPath); break;
			case SmoothType.CurvedNonuniform:
				path = CurvedNonuniform(p.vectorPath); break;
			}

			if (path != p.vectorPath) {
				ListPool<Vector3>.Release(ref p.vectorPath);
				p.vectorPath = path;
			}
		}

		public List<Vector3> CurvedNonuniform (List<Vector3> path) {
			if (maxSegmentLength <= 0) {
				Debug.LogWarning("Max Segment Length is <= 0 which would cause DivByZero-exception or other nasty errors (avoid this)");
				return path;
			}

			int pointCounter = 0;
			for (int i = 0; i < path.Count-1; i++) {
				//pointCounter += Mathf.FloorToInt ((path[i]-path[i+1]).magnitude / maxSegmentLength)+1;

				float dist = (path[i]-path[i+1]).magnitude;
				//In order to avoid floating point errors as much as possible, and in lack of a better solution
				//loop through it EXACTLY as the other code further down will
				for (float t = 0; t <= dist; t += maxSegmentLength) {
					pointCounter++;
				}
			}

			List<Vector3> subdivided = ListPool<Vector3>.Claim(pointCounter);

			// Set first velocity
			Vector3 preEndVel = (path[1]-path[0]).normalized;

			for (int i = 0; i < path.Count-1; i++) {
				float dist = (path[i]-path[i+1]).magnitude;

				Vector3 startVel1 = preEndVel;
				Vector3 endVel1 = i < path.Count-2 ? ((path[i+2]-path[i+1]).normalized - (path[i]-path[i+1]).normalized).normalized : (path[i+1]-path[i]).normalized;

				Vector3 startVel = startVel1 * dist * factor;
				Vector3 endVel = endVel1 * dist * factor;

				Vector3 start = path[i];
				Vector3 end = path[i+1];

				float onedivdist = 1F / dist;

				for (float t = 0; t <= dist; t += maxSegmentLength) {
					float t2 = t * onedivdist;

					subdivided.Add(GetPointOnCubic(start, end, startVel, endVel, t2));
				}

				preEndVel = endVel1;
			}

			subdivided[subdivided.Count-1] = path[path.Count-1];

			return subdivided;
		}

		public static Vector3 GetPointOnCubic (Vector3 a, Vector3 b, Vector3 tan1, Vector3 tan2, float t) {
			float t2 = t*t, t3 = t2*t;

			float h1 =  2*t3 - 3*t2 + 1;          // calculate basis function 1
			float h2 = -2*t3 + 3*t2;              // calculate basis function 2
			float h3 =   t3 -  2*t2 + t;          // calculate basis function 3
			float h4 =   t3 -  t2;                // calculate basis function 4

			return h1*a +                            // multiply and sum all funtions
				   h2*b +                            // together to build the interpolated
				   h3*tan1 +                         // point along the curve.
				   h4*tan2;
		}

		public List<Vector3> SmoothOffsetSimple (List<Vector3> path) {
			if (path.Count <= 2 || iterations <= 0) {
				return path;
			}

			if (iterations > 12) {
				Debug.LogWarning("A very high iteration count was passed, won't let this one through");
				return path;
			}

			int maxLength = (path.Count-2)*(int)Mathf.Pow(2, iterations)+2;

			List<Vector3> subdivided = ListPool<Vector3>.Claim(maxLength);
			List<Vector3> subdivided2 = ListPool<Vector3>.Claim(maxLength);

			for (int i = 0; i < maxLength; i++) { subdivided.Add(Vector3.zero); subdivided2.Add(Vector3.zero); }

			for (int i = 0; i < path.Count; i++) {
				subdivided[i] = path[i];
			}

			for (int iteration = 0; iteration < iterations; iteration++) {
				int currentPathLength = (path.Count-2)*(int)Mathf.Pow(2, iteration)+2;

				//Switch the arrays
				List<Vector3> tmp = subdivided;
				subdivided = subdivided2;
				subdivided2 = tmp;

				const float nextMultiplier = 1F;

				for (int i = 0; i < currentPathLength-1; i++) {
					Vector3 current = subdivided2[i];
					Vector3 next = subdivided2[i+1];

					Vector3 normal = Vector3.Cross(next-current, Vector3.up);
					normal = normal.normalized;

					bool firstRight = false;
					bool secondRight = false;
					bool setFirst = false;
					bool setSecond = false;
					if (i != 0 && !VectorMath.IsColinearXZ(current, next, subdivided2[i-1])) {
						setFirst = true;
						firstRight = VectorMath.RightOrColinearXZ(current, next, subdivided2[i-1]);
					}
					if (i < currentPathLength-1 && !VectorMath.IsColinearXZ(current, next, subdivided2[i+2])) {
						setSecond = true;
						secondRight = VectorMath.RightOrColinearXZ(current, next, subdivided2[i+2]);
					}

					if (setFirst) {
						subdivided[i*2] = current + (firstRight ? normal*offset*nextMultiplier : -normal*offset*nextMultiplier);
					} else {
						subdivided[i*2] = current;
					}

					if (setSecond) {
						subdivided[i*2+1] = next  + (secondRight ? normal*offset*nextMultiplier : -normal*offset*nextMultiplier);
					} else {
						subdivided[i*2+1] = next;
					}
				}

				subdivided[(path.Count-2)*(int)Mathf.Pow(2, iteration+1)+2-1] = subdivided2[currentPathLength-1];
			}

			ListPool<Vector3>.Release(ref subdivided2);

			return subdivided;
		}

		public List<Vector3> SmoothSimple (List<Vector3> path) {
			if (path.Count < 2) return path;

			List<Vector3> subdivided;

			if (uniformLength) {
				// Clamp to a small value to avoid the path being divided into a huge number of segments
				maxSegmentLength = Mathf.Max(maxSegmentLength, 0.005f);

				float pathLength = 0;
				for (int i = 0; i < path.Count-1; i++) {
					pathLength += Vector3.Distance(path[i], path[i+1]);
				}

				int estimatedNumberOfSegments = Mathf.FloorToInt(pathLength / maxSegmentLength);
				// Get a list with an initial capacity high enough so that we can add all points
				subdivided = ListPool<Vector3>.Claim(estimatedNumberOfSegments+2);

				float distanceAlong = 0;

				// Sample points every [maxSegmentLength] world units along the path
				for (int i = 0; i < path.Count-1; i++) {
					var start = path[i];
					var end = path[i+1];

					float length = Vector3.Distance(start, end);

					while (distanceAlong < length) {
						subdivided.Add(Vector3.Lerp(start, end, distanceAlong / length));
						distanceAlong += maxSegmentLength;
					}

					distanceAlong -= length;
				}

				// Make sure we get the exact position of the last point
				subdivided.Add(path[path.Count-1]);
			} else {
				subdivisions = Mathf.Max(subdivisions, 0);

				if (subdivisions > 10) {
					Debug.LogWarning("Very large number of subdivisions. Cowardly refusing to subdivide every segment into more than " + (1 << subdivisions) + " subsegments");
					subdivisions = 10;
				}

				int steps = 1 << subdivisions;
				subdivided = ListPool<Vector3>.Claim((path.Count-1)*steps + 1);
				Polygon.Subdivide(path, subdivided, steps);
			}

			if (strength > 0) {
				for (int it = 0; it < iterations; it++) {
					Vector3 prev = subdivided[0];

					for (int i = 1; i < subdivided.Count-1; i++) {
						Vector3 tmp = subdivided[i];

						// prev is at this point set to the value that subdivided[i-1] had before this loop started
						// Move the point closer to the average of the adjacent points
						subdivided[i] = Vector3.Lerp(tmp, (prev+subdivided[i+1])/2F, strength);

						prev = tmp;
					}
				}
			}

			return subdivided;
		}

		public List<Vector3> SmoothBezier (List<Vector3> path) {
			if (subdivisions < 0) subdivisions = 0;

			int subMult = 1 << subdivisions;
			List<Vector3> subdivided = ListPool<Vector3>.Claim();

			for (int i = 0; i < path.Count-1; i++) {
				Vector3 tangent1;
				Vector3 tangent2;
				if (i == 0) {
					tangent1 = path[i+1]-path[i];
				} else {
					tangent1 = path[i+1]-path[i-1];
				}

				if (i == path.Count-2) {
					tangent2 = path[i]-path[i+1];
				} else {
					tangent2 = path[i]-path[i+2];
				}

				tangent1 *= bezierTangentLength;
				tangent2 *= bezierTangentLength;

				Vector3 v1 = path[i];
				Vector3 v2 = v1+tangent1;
				Vector3 v4 = path[i+1];
				Vector3 v3 = v4+tangent2;

				for (int j = 0; j < subMult; j++) {
					subdivided.Add(AstarSplines.CubicBezier(v1, v2, v3, v4, (float)j/subMult));
				}
			}

			// Assign the last point
			subdivided.Add(path[path.Count-1]);

			return subdivided;
		}
	}
}