CapersProject/Packages/com.arongranberg.astar/Navmesh/NavmeshCut.cs

using UnityEngine;
using System.Collections.Generic;

namespace Pathfinding {
	using Pathfinding.Pooling;
	using Pathfinding.Drawing;
	using Pathfinding.Util;
	using Pathfinding.Collections;
	using Unity.Burst;
	using Unity.Collections;
	using Unity.Mathematics;
	using Unity.Collections.LowLevel.Unsafe;

	/// <summary>
	/// Navmesh cutting is used for fast recast/navmesh graph updates.
	///
	/// Navmesh cutting is used to cut holes in an existing navmesh generated by a recast or navmesh graph.
	/// With navmesh cutting you can remove (cut) parts of the navmesh that is blocked by obstacles such as a new building in an RTS game however you cannot add anything new to the navmesh or change
	/// the positions of the nodes.
	///
	/// Normal graph updates on recast/navmesh graphs, in contrast, only allow either just changing parameters on existing nodes (e.g make a whole triangle unwalkable), which is not very flexible, or recalculate whole tiles, which can be slow.
	/// Navmesh cutting is typically significantly faster than recalculating whole tiles from scratch in a recast graph.
	///
	/// [Open online documentation to see videos]
	///
	/// The NavmeshCut component uses a 2D shape to cut the navmesh with. This shape can be produced by either one of the built-in 2D shapes (rectangle/circle) or one of the 3D shapes (cube/sphere/capsule)
	/// which will be projected down to a 2D shape when cutting happens. You can also specify a custom 2D mesh to use as a cut.
	///
	/// [Open online documentation to see images]
	///
	/// Note that the rectangle/circle shapes are not 3D. If you rotate them, you will see that the 2D shape will be rotated and then just projected down on the XZ plane.
	/// Therefore it is recommended to use the 3D shapes (cube/sphere/capsule) in most cases since those are easier to use.
	///
	/// In the scene view the NavmeshCut looks like an extruded 2D shape because a navmesh cut also has a height. It will only cut the part of the
	/// navmesh which it touches. For performance reasons it only checks the bounding boxes of the triangles in the navmesh, so it may cut triangles
	/// whoose bounding boxes it intersects even if the triangle does not intersect the extruded shape. However in most cases this does not make a large difference.
	///
	/// It is also possible to set the navmesh cut to dual mode by setting the <see cref="isDual"/> field to true. This will prevent it from cutting a hole in the navmesh
	/// and it will instead just split the navmesh along the border but keep both the interior and the exterior. This can be useful if you for example
	/// want to change the penalty of some region which does not neatly line up with the navmesh triangles. It is often combined with the GraphUpdateScene component
	/// (however note that the GraphUpdateScene component will not automatically reapply the penalty if the graph is updated again).
	///
	/// By default the navmesh cut does not take rotation or scaling into account. If you want to do that, you can set the <see cref="useRotationAndScale"/> field to true.
	///
	/// <b>Custom meshes</b>
	/// For most purposes you can use the built-in shapes, however in some cases a custom cutting mesh may be useful.
	/// The custom mesh should be a flat 2D shape like in the image below. The script will then find the contour of that mesh and use that shape as the cut.
	/// Make sure that all normals are smooth and that the mesh contains no UV information. Otherwise Unity might split a vertex and then the script will not
	/// find the correct contour. You should not use a very high polygon mesh since that will create a lot of nodes in the navmesh graph and slow
	/// down pathfinding because of that. For very high polygon meshes it might even cause more suboptimal paths to be generated if it causes many
	/// thin triangles to be added to the navmesh.
	/// [Open online documentation to see images]
	///
	/// <b>Update frequency</b>
	///
	/// Navmesh cuts are typically pretty fast, so you may be tempted to make them update the navmesh very often (once every few frames perhaps), just because you can spare the CPU power.
	/// However, updating the navmesh too often can also have consequences for agents that are following paths on the graph.
	///
	/// If a navmesh cut updates the graph near an agent, it will usually have to recalculate its path. If this happens too often, this can lead to the pathfinding
	/// worker threads being overwhelmed by pathfinding requests, causing higher latency for individual pathfinding requests. This can, in turn, make agents less responsive.
	///
	/// So it's recommended to keep the update frequency reasonable. After all, a player is unlikely to notice if the navmesh was updated 20 times per second or 2 times per second (or even less often).
	///
	/// You can primarily control this using the <see cref="updateDistance"/> and <see cref="updateRotationDistance"/> fields. But you can also control the global frequency of updates. This is explained in the next section.
	///
	/// <b>Control updates through code</b>
	/// Navmesh cuts are applied periodically, but sometimes you may want to ensure the graph is up to date right now.
	/// Then you can use the following code.
	/// <code>
	/// // Schedule pending updates to be done as soon as the pathfinding threads
	/// // are done with what they are currently doing.
	/// AstarPath.active.navmeshUpdates.ForceUpdate();
	/// // Block until the updates have finished
	/// AstarPath.active.FlushGraphUpdates();
	/// </code>
	///
	/// You can also control how often the scripts check for if any navmesh cut has changed.
	/// If you have a very large number of cuts it may be good for performance to not check it as often.
	/// <code>
	/// // Check every frame (the default)
	/// AstarPath.active.navmeshUpdates.updateInterval = 0;
	///
	/// // Check every 0.1 seconds
	/// AstarPath.active.navmeshUpdates.updateInterval = 0.1f;
	///
	/// // Never check for changes
	/// AstarPath.active.navmeshUpdates.updateInterval = -1;
	/// // You will have to schedule updates manually using
	/// AstarPath.active.navmeshUpdates.ForceUpdate();
	/// </code>
	///
	/// You can also find this setting in the AstarPath inspector under Settings.
	/// [Open online documentation to see images]
	///
	/// <b>Navmesh cutting and tags/penalties</b>
	/// Navmesh cuts can only preserve tags for updates which happen when the graph is first scanned, or when a recast graph tile is recalculated from scratch.
	///
	/// This means that any tags that you apply dynamically using e.g. a <see cref="GraphUpdateScene"/> component may be lost when a navmesh cut is applied.
	/// If you need to combine tags and navmesh cutting, it is therefore strongly recommended to use the <see cref="RecastNavmeshModifier"/> component to apply the tags,
	/// as that will work smoothly with navmesh cutting.
	///
	/// Internally, what happens is that when a graph is scanned, the navmesh cutting subsystem takes a snapshot of all triangles in the graph, including tags. This data will then be referenced
	/// every time cutting happens and the tags from the snapshot will be copied to the new triangles after cutting has taken place.
	///
	/// You can also apply tags and penalties using a graph update after cutting has taken place. For example by subclassing a navmesh cut and overriding the <see cref="UsedForCut"/> method.
	/// However, it is recommended to use the <see cref="RecastNavmeshModifier"/> as mentioned before, as this is a more robust solution.
	///
	/// See: http://www.arongranberg.com/2013/08/navmesh-cutting/
	///
	/// Version: This component is only available in 2022.3 and later, due to Unity bugs in earlier versions.
	/// </summary>
	[AddComponentMenu("Pathfinding/Navmesh/Navmesh Cut")]
	[ExecuteAlways]
	[HelpURL("https://arongranberg.com/astar/documentation/stable/navmeshcut.html")]
	public class NavmeshCut : NavmeshClipper {
		public enum MeshType {
			/// <summary>A 2D rectangle</summary>
			Rectangle,
			/// <summary>A 2D circle</summary>
			Circle,
			CustomMesh,
			/// <summary>A 3D box which will be projected down to a 2D outline</summary>
			Box,
			/// <summary>A 3D sphere which will be projected down to a 2D outline</summary>
			Sphere,
			/// <summary>A 3D capsule which will be projected down to a 2D outline</summary>
			Capsule,
		}

		public enum RadiusExpansionMode {
			/// <summary>
			/// If DontExpand is used then the cut will be exactly as specified with no modifications.
			/// It will be the same for all graphs.
			/// </summary>
			DontExpand,
			/// <summary>
			/// If ExpandByAgentRadius is used then the cut will be expanded by the agent's radius (set in the recast graph settings)
			/// in every direction. For navmesh graphs (which do not have a character radius) this is equivalent to DontExpand.
			///
			/// This is especially useful if you have multiple graphs for different unit sizes and want the cuts to be sized according to
			/// the different units.
			/// </summary>
			ExpandByAgentRadius,
		}

		/// <summary>Shape of the cut</summary>
		[Tooltip("Shape of the cut")]
		public MeshType type = MeshType.Box;

		/// <summary>
		/// Custom mesh to use.
		/// The contour(s) of the mesh will be extracted.
		/// If you get the "max perturbations" error when cutting with this, check the normals on the mesh.
		/// They should all point in the same direction. Try flipping them if that does not help.
		///
		/// This mesh should only be a 2D surface, not a volume.
		/// </summary>
		[Tooltip("The contour(s) of the mesh will be extracted. This mesh should only be a 2D surface, not a volume (see documentation).")]
		public Mesh mesh;

		/// <summary>Size of the rectangle</summary>
		public Vector2 rectangleSize = new Vector2(1, 1);

		/// <summary>Radius of the circle</summary>
		public float circleRadius = 1;

		/// <summary>Number of vertices on the circle</summary>
		public int circleResolution = 6;

		/// <summary>The cut will be extruded to this height</summary>
		public float height = 1;

		/// <summary>Scale of the custom mesh, if used</summary>
		[Tooltip("Scale of the custom mesh")]
		public float meshScale = 1;

		public Vector3 center;

		/// <summary>
		/// How much the cut must move before the navmesh is updated.
		/// A smaller distance gives better accuracy, but requires more updates when moving the object over time,
		/// so it is often slower.
		///
		/// Even if the graph update itself is fast, having a low value can make agents have to recalculate their paths a lot more often,
		/// leading to lower performance.
		/// </summary>
		[Tooltip("Distance between positions to require an update of the navmesh\nA smaller distance gives better accuracy, but requires more updates when moving the object over time, so it is often slower.")]
		public float updateDistance = 0.4f;

		/// <summary>
		/// Only makes a split in the navmesh, but does not remove the geometry to make a hole.
		/// This is slower than a normal cut
		/// </summary>
		[Tooltip("Only makes a split in the navmesh, but does not remove the geometry to make a hole")]
		public bool isDual;

		/// <summary>
		/// If the cut should be expanded by the agent radius or not.
		///
		/// See <see cref="RadiusExpansionMode"/> for more details.
		/// </summary>

		public RadiusExpansionMode radiusExpansionMode = RadiusExpansionMode.ExpandByAgentRadius;

		/// <summary>
		/// Cuts geometry added by a NavmeshAdd component.
		/// You rarely need to change this
		/// </summary>
		public bool cutsAddedGeom = true;

		/// <summary>
		/// How many degrees the object must rotate before the navmesh is updated.
		/// Should be between 0 and 180.
		/// </summary>
		[Tooltip("How many degrees rotation that is required for an update to the navmesh. Should be between 0 and 180.")]
		public float updateRotationDistance = 10;

		/// <summary>
		/// Includes rotation and scale in calculations.
		///
		/// If this is disabled, the object's rotation and scale is not taken into account when determining the shape of the cut.
		///
		/// Enabling this is a bit slower, since a lot more matrix multiplications are needed.
		/// </summary>
		[Tooltip("Includes rotation in calculations. This is slower since a lot more matrix multiplications are needed but gives more flexibility.")]
		[UnityEngine.Serialization.FormerlySerializedAsAttribute("useRotation")]
		public bool useRotationAndScale;

		NativeList<float3> meshContourVertices;
		NativeList<ContourBurst> meshContours;

		/// <summary>cached transform component</summary>
		protected Transform tr;
		Mesh lastMesh;

		protected override void Awake () {
			base.Awake();
			tr = transform;
		}

		protected override void OnDisable () {
			// This needs to run in the editor as well which is why it is in OnDisable.
			// OnDestroy will not necessarily get called in editor mode.
			if (this.meshContourVertices.IsCreated) this.meshContourVertices.Dispose();
			if (this.meshContours.IsCreated) this.meshContours.Dispose();
			lastMesh = null;

			base.OnDisable();
		}

		/// <summary>Cached variable, to avoid allocations</summary>
		static readonly Dictionary<Vector2Int, int> edges = new Dictionary<Vector2Int, int>();
		/// <summary>Cached variable, to avoid allocations</summary>
		static readonly Dictionary<int, int> pointers = new Dictionary<int, int>();

		/// <summary>
		/// Forces this navmesh cut to update the navmesh.
		///
		/// This update is not instant, it is done the next time it is checked if it needs updating.
		/// See: <see cref="NavmeshUpdates.updateInterval"/>
		/// See: <see cref="NavmeshUpdates.ForceUpdate"/>
		/// </summary>
		public override void ForceUpdate () {
			if (AstarPath.active != null) AstarPath.active.navmeshUpdates.ForceUpdateAround(this);
		}

		/// <summary>
		/// Returns true if this object has moved so much that it requires an update.
		/// When an update to the navmesh has been done, call NotifyUpdated to be able to get
		/// relavant output from this method again.
		/// </summary>
		public override bool RequiresUpdate (GridLookup<NavmeshClipper>.Root previousState) {
			return (tr.position-previousState.previousPosition).sqrMagnitude > updateDistance*updateDistance || (useRotationAndScale && (Quaternion.Angle(previousState.previousRotation, tr.rotation) > updateRotationDistance));
		}

		/// <summary>
		/// Called whenever this navmesh cut is used to update the navmesh.
		/// Called once for each tile the navmesh cut is in.
		/// You can override this method to execute custom actions whenever this happens.
		/// </summary>
		public virtual void UsedForCut () {
		}

		/// <summary>Internal method to notify the NavmeshCut that it has just been used to update the navmesh</summary>
		public override void NotifyUpdated (GridLookup<NavmeshClipper>.Root previousState) {
			previousState.previousPosition = tr.position;

			if (useRotationAndScale) {
				previousState.previousRotation = tr.rotation;
			}
		}

		void CalculateMeshContour () {
			if (mesh == null) return;

			edges.Clear();
			pointers.Clear();

			Vector3[] verts = mesh.vertices;
			int[] tris = mesh.triangles;
			for (int i = 0; i < tris.Length; i += 3) {
				// Make sure it is clockwise
				if (VectorMath.IsClockwiseXZ(verts[tris[i+0]], verts[tris[i+1]], verts[tris[i+2]])) {
					int tmp = tris[i+0];
					tris[i+0] = tris[i+2];
					tris[i+2] = tmp;
				}

				edges[new Vector2Int(tris[i+0], tris[i+1])] = i;
				edges[new Vector2Int(tris[i+1], tris[i+2])] = i;
				edges[new Vector2Int(tris[i+2], tris[i+0])] = i;
			}

			// Construct a list of pointers along all edges
			for (int i = 0; i < tris.Length; i += 3) {
				for (int j = 0; j < 3; j++) {
					if (!edges.ContainsKey(new Vector2Int(tris[i+((j+1)%3)], tris[i+((j+0)%3)]))) {
						pointers[tris[i+((j+0)%3)]] = tris[i+((j+1)%3)];
					}
				}
			}

			var contourVertexBuffer = new NativeList<float3>(Allocator.Persistent);
			var contourBuffer = new NativeList<ContourBurst>(Allocator.Persistent);

			// Follow edge pointers to generate the contours
			for (int i = 0; i < verts.Length; i++) {
				if (pointers.ContainsKey(i)) {
					var startIndex = contourVertexBuffer.Length;

					int s = i;
					do {
						int tmp = pointers[s];

						//This path has been taken before
						if (tmp == -1) break;

						pointers[s] = -1;
						contourVertexBuffer.Add(verts[s]);
						s = tmp;
					} while (s != i);

					if (contourVertexBuffer.Length != startIndex) {
						contourBuffer.Add(new ContourBurst {
							startIndex = startIndex,
							endIndex = contourVertexBuffer.Length,
							ymin = 0,
							ymax = 0,
						});
					}
				}
			}

			if (this.meshContourVertices.IsCreated) this.meshContourVertices.Dispose();
			if (this.meshContours.IsCreated) this.meshContours.Dispose();
			this.meshContourVertices = contourVertexBuffer;
			this.meshContours = contourBuffer;
		}

		/// <summary>
		/// Bounds in XZ space after transforming using the *inverse* transform of the inverseTransform parameter.
		/// The transformation will typically transform the vertices to graph space and this is used to
		/// figure out which tiles the cut intersects.
		/// </summary>
		public override Rect GetBounds (GraphTransform inverseTransform, float radiusMargin) {
			var buffers = ListPool<Contour>.Claim();

			GetContour(buffers, inverseTransform.inverseMatrix, radiusMargin);

			Rect r = new Rect();
			for (int i = 0; i < buffers.Count; i++) {
				var buffer = buffers[i].contour;
				for (int k = 0; k < buffer.Count; k++) {
					var p = buffer[k];
					if (k == 0 && i == 0) {
						r = new Rect(p.x, p.y, 0, 0);
					} else {
						r.xMax = System.Math.Max(r.xMax, p.x);
						r.yMax = System.Math.Max(r.yMax, p.y);
						r.xMin = System.Math.Min(r.xMin, p.x);
						r.yMin = System.Math.Min(r.yMin, p.y);
					}
				}
				ListPool<Vector2>.Release(ref buffer);
			}

			ListPool<Contour>.Release(ref buffers);
			return r;
		}

		public struct Contour {
			public float ymin;
			public float ymax;
			public List<Vector2> contour;
		}

		public struct ContourBurst {
			public int startIndex;
			public int endIndex;
			public float ymin;
			public float ymax;
		}

		Matrix4x4 contourTransformationMatrix {
			get {
				// Take rotation and scaling into account
				if (useRotationAndScale) {
					return tr.localToWorldMatrix * Matrix4x4.Translate(center);
				} else {
					return Matrix4x4.Translate(tr.position + center);
				}
			}
		}

		/// <summary>
		/// Contour of the navmesh cut.
		/// Fills the specified buffer with all contours.
		/// The cut may contain several contours which is why the buffer is a list of lists.
		/// </summary>
		/// <param name="buffer">Will be filled with the result</param>
		/// <param name="matrix">All points will be transformed using this matrix. They are in world space before the transformation. Typically this a transform that maps from world space to graph space.</param>
		/// <param name="radiusMargin">The obstacle will be expanded by this amount. Typically this is the character radius for the graph. The MeshType.CustomMesh does not support this.
		/// If #radiusExpansionMode is RadiusExpansionMode.DontExpand then this parameter is ignored.</param>
		public void GetContour (List<Contour> buffer, Matrix4x4 matrix, float radiusMargin) {
			var outputVertices = new UnsafeList<float2>(0, Allocator.Temp);
			var outputContours = new UnsafeList<ContourBurst>(1, Allocator.Temp);

			unsafe {
				GetContourBurst(&outputVertices, &outputContours, matrix, radiusMargin);
			}
			for (int i = 0; i < outputContours.Length; i++) {
				var list = ListPool<Vector2>.Claim();

				var contour = outputContours[i];
				for (int j = contour.startIndex; j < contour.endIndex; j++) {
					list.Add(outputVertices[j]);
				}
				buffer.Add(new Contour {
					ymin = contour.ymin,
					ymax = contour.ymax,
					contour = list,
				});
			}
			outputVertices.Dispose();
			outputContours.Dispose();
		}

		/// <summary>
		/// Contour of the navmesh cut.
		/// Fills the specified buffer with all contours.
		/// The cut may contain several contours.
		/// </summary>
		/// <param name="outputVertices">Will be filled with all vertices</param>
		/// <param name="outputContours">Will be filled with all contours that reference the outputVertices list.</param>
		/// <param name="matrix">All points will be transformed using this matrix. They are in world space before the transformation. Typically this a matrix that maps from world space to graph space.</param>
		/// <param name="radiusMargin">The obstacle will be expanded by this amount. Typically this is the character radius for the graph. The MeshType.CustomMesh does not support this.</param>
		public unsafe void GetContourBurst (UnsafeList<float2>* outputVertices, UnsafeList<ContourBurst>* outputContours, Matrix4x4 matrix, float radiusMargin) {
			if (radiusExpansionMode == RadiusExpansionMode.DontExpand) {
				radiusMargin = 0;
			}

			if (type == MeshType.CustomMesh && (mesh != lastMesh || !meshContours.IsCreated || !meshContourVertices.IsCreated)) {
				CalculateMeshContour();
				lastMesh = mesh;
			}

			var job = new NavmeshCutJobs.JobCalculateContour {
				outputVertices = outputVertices,
				outputContours = outputContours,
				matrix = matrix,
				localToWorldMatrix = contourTransformationMatrix,
				radiusMargin = radiusMargin,
				circleResolution = circleResolution,
				circleRadius = circleRadius,
				rectangleSize = rectangleSize,
				height = height,
				meshType = type,
				meshContours = (UnsafeList<ContourBurst>*) this.meshContours.GetUnsafeList(),
				meshContourVertices = (UnsafeList<float3>*) this.meshContourVertices.GetUnsafeList(),
				meshScale = meshScale,
			};

			NavmeshCutJobs.CalculateContour(ref job);
		}

		public static readonly Color GizmoColor = new Color(37.0f/255, 184.0f/255, 239.0f/255);
		public static readonly Color GizmoColor2 = new Color(169.0f/255, 92.0f/255, 242.0f/255);

		public override void DrawGizmos () {
			if (tr == null) tr = transform;

			bool selected = GizmoContext.InActiveSelection(tr);

			var graph = AstarPath.active != null ? (AstarPath.active.data.recastGraph as NavmeshBase ?? AstarPath.active.data.navmeshGraph) : null;
			var matrix = graph != null ? graph.transform : GraphTransform.identityTransform;

			var characterRadius = graph != null ? graph.NavmeshCuttingCharacterRadius : 0;
			var contourVertices = new UnsafeList<float2>(0, Allocator.Temp);
			var contours = new UnsafeList<NavmeshCut.ContourBurst>(0, Allocator.Temp);
			unsafe {
				GetContourBurst(&contourVertices, &contours, matrix.inverseMatrix, characterRadius);
			}

			var col = Color.Lerp(GizmoColor, Color.white, 0.5f);
			col.a *= 0.5f;
			using (Draw.WithColor(col)) {
				// Draw all contours
				for (int i = 0; i < contours.Length; i++) {
					var contour = contours[i];
					var ymid = (contour.ymin + contour.ymax)*0.5f;
					var count = contour.endIndex - contour.startIndex;
					for (int j = 0; j < count; j++) {
						var v1 = contourVertices[contour.startIndex + j];
						var v2 = contourVertices[contour.startIndex + (j+1) % count];
						var p1 = new Vector3(v1.x, ymid, v1.y);
						var p2 = new Vector3(v2.x, ymid, v2.y);
						// Note: Drawn with a stronger color
						Draw.Line(matrix.Transform(p1), matrix.Transform(p2), GizmoColor);

						if (selected) {
							Vector3 p1low = p1, p2low = p2, p1high = p1, p2high = p2;
							p1low.y = p2low.y = contour.ymin;
							p1high.y = p2high.y = contour.ymax;

							Draw.Line(matrix.Transform(p1low), matrix.Transform(p2low));
							Draw.Line(matrix.Transform(p1high), matrix.Transform(p2high));
							Draw.Line(matrix.Transform(p1low), matrix.Transform(p1high));
						}
					}
				}
			}

			if (selected) {
				switch (type) {
				case MeshType.Box:
					using (Draw.WithMatrix(contourTransformationMatrix * Matrix4x4.Scale(new Vector3(rectangleSize.x, height, rectangleSize.y)))) {
						Draw.WireBox(Vector3.zero, Vector3.one, GizmoColor2);
					}
					break;
				case MeshType.Capsule: {
					var m = contourTransformationMatrix;
					var height = Mathf.Max(this.height, circleRadius * 2);
					var scaleFactorX = math.length(m.GetColumn(0));
					var scaleFactorZ = math.length(m.GetColumn(2));
					var radius = this.circleRadius * math.max(scaleFactorX, scaleFactorZ);
					var mainAxis = ((Vector3)m.GetColumn(1)).normalized;
					var hemispherePos1 = contourTransformationMatrix.MultiplyPoint3x4(new Vector3(0, height*0.5f, 0)) - mainAxis * radius;
					var hemispherePos2 = contourTransformationMatrix.MultiplyPoint3x4(-new Vector3(0, height*0.5f, 0)) + mainAxis * radius;
					Draw.WireCapsule(hemispherePos1, hemispherePos2, radius, GizmoColor2);
					break;
				}
				case MeshType.Sphere: {
					var uniformScaleFactor = useRotationAndScale ? math.cmax(tr.lossyScale) : 1;
					var sphereMatrix = Matrix4x4.TRS(tr.position, useRotationAndScale ? tr.rotation : Quaternion.identity, Vector3.one * uniformScaleFactor) * Matrix4x4.Translate(center);
					using (Draw.WithMatrix(sphereMatrix)) {
						Draw.WireSphere(Vector3.zero, circleRadius, GizmoColor2);
					}
					break;
				}
				case MeshType.CustomMesh: {
					if (mesh != null) {
						using (Draw.WithMatrix(contourTransformationMatrix * Matrix4x4.Scale(Vector3.one * meshScale))) {
							Draw.WireMesh(mesh, GizmoColor2);
						}
					}
					break;
				}
				}
			}

			contourVertices.Dispose();
			contours.Dispose();
		}

		protected override void OnUpgradeSerializedData (ref Serialization.Migrations migrations, bool unityThread) {
			if (migrations.TryMigrateFromLegacyFormat(out var legacyVersion)) {
				if (legacyVersion < 2) {
					this.radiusExpansionMode = RadiusExpansionMode.DontExpand;
				}
			}
		}
	}

	[BurstCompile]
	internal static class NavmeshCutJobs {
		[BurstCompile(FloatPrecision.Standard, FloatMode.Fast)]
		public static unsafe void CalculateContour (ref JobCalculateContour job) {
			job.Execute();
		}

		static readonly float4[] BoxCorners = new float4[] {
			new float4(-0.5f, -0.5f, -0.5f, 1.0f),
			new float4(+0.5f, -0.5f, -0.5f, 1.0f),
			new float4(-0.5f, +0.5f, -0.5f, 1.0f),
			new float4(+0.5f, +0.5f, -0.5f, 1.0f),
			new float4(-0.5f, -0.5f, +0.5f, 1.0f),
			new float4(+0.5f, -0.5f, +0.5f, 1.0f),
			new float4(-0.5f, +0.5f, +0.5f, 1.0f),
			new float4(+0.5f, +0.5f, +0.5f, 1.0f),
		};

		public struct JobCalculateContour {
			public unsafe Unity.Collections.LowLevel.Unsafe.UnsafeList<float2>* outputVertices;
			public unsafe Unity.Collections.LowLevel.Unsafe.UnsafeList<NavmeshCut.ContourBurst>* outputContours;
			public unsafe Unity.Collections.LowLevel.Unsafe.UnsafeList<NavmeshCut.ContourBurst>* meshContours;
			public unsafe Unity.Collections.LowLevel.Unsafe.UnsafeList<float3>* meshContourVertices;
			public float4x4 matrix;
			public float4x4 localToWorldMatrix;
			public float radiusMargin;
			public int circleResolution;
			public float circleRadius;
			public float2 rectangleSize;
			public float height;
			public float meshScale;
			public NavmeshCut.MeshType meshType;

			public unsafe void Execute () {
				circleResolution = math.max(circleResolution, 3);

				// Take rotation and scaling into account
				var localToGraphMatrix = math.mul(this.matrix, this.localToWorldMatrix);

				// radiusMargin should not be affected by the matrices at all. So we need to compensate for that.
				var scaleFactorX = math.length(localToGraphMatrix.c0);
				var scaleFactorY = math.length(localToGraphMatrix.c1);
				var scaleFactorZ = math.length(localToGraphMatrix.c2);

				switch (meshType) {
				case NavmeshCut.MeshType.Rectangle: {
					rectangleSize = new float2(math.abs(rectangleSize.x), math.abs(rectangleSize.y)) + math.rcp(new float2(scaleFactorX, scaleFactorZ))*radiusMargin*2;
					outputVertices->Add(math.transform(localToGraphMatrix, new float3(-rectangleSize.x, 0, -rectangleSize.y)*0.5f).xz);
					outputVertices->Add(math.transform(localToGraphMatrix, new float3(rectangleSize.x, 0, -rectangleSize.y)*0.5f).xz);
					outputVertices->Add(math.transform(localToGraphMatrix, new float3(rectangleSize.x, 0, rectangleSize.y)*0.5f).xz);
					outputVertices->Add(math.transform(localToGraphMatrix, new float3(-rectangleSize.x, 0, rectangleSize.y)*0.5f).xz);

					// matrix.c3.y is just the y coordinate of the translation part of the matrix
					var y0 = localToGraphMatrix.c3.y;
					outputContours->Add(new NavmeshCut.ContourBurst {
							// Make sure the height of the cut is also increased if the user scales the object along the y axis
							ymin = y0 - this.height * 0.5f * scaleFactorY,
							ymax = y0 + this.height * 0.5f * scaleFactorY,
							startIndex = outputVertices->Length - 4,
							endIndex = outputVertices->Length,
						});
					break;
				}
				case NavmeshCut.MeshType.Sphere: {
					circleRadius = math.abs(circleRadius);

					// For a sphere we ignore all rotation and non-uniform scaling.
					// Instead we only use the translation part of the localToWorldMatrix
					localToGraphMatrix = math.mul(this.matrix, float4x4.Translate(this.localToWorldMatrix.c3.xyz));
					// Then we scale using a uniform scaling.
					// This corresponds to what for example the unity sphere collider does when the object is scaled
					// using a non-uniform scale.
					var uniformScaleFactor = math.max(scaleFactorX, math.max(scaleFactorY, scaleFactorZ));
					scaleFactorX = scaleFactorY = scaleFactorZ = uniformScaleFactor;

					localToGraphMatrix = math.mul(localToGraphMatrix, float4x4.Scale(uniformScaleFactor));
					var radius = circleRadius + radiusMargin/uniformScaleFactor;
					radius = ApproximateCircleWithPolylineRadius(radius, circleResolution);

					var step = (2*Mathf.PI)/circleResolution;

					for (int i = 0; i < circleResolution; i++) {
						math.sincos(i*step, out float sin, out float cos);
						outputVertices->Add(math.transform(localToGraphMatrix, new float3(cos * radius, 0, sin*radius)).xz);
					}

					var y0 = localToGraphMatrix.c3.y;
					outputContours->Add(new NavmeshCut.ContourBurst {
							ymin = y0 - radius * uniformScaleFactor,
							ymax = y0 + radius * uniformScaleFactor,
							startIndex = outputVertices->Length - circleResolution,
							endIndex = outputVertices->Length,
						});
					break;
				}
				case NavmeshCut.MeshType.Circle: {
					circleRadius = math.abs(circleRadius);

					var height = this.height + radiusMargin/scaleFactorY;
					var radiusX = circleRadius + radiusMargin/scaleFactorX;
					var radiusZ = circleRadius + radiusMargin/scaleFactorZ;

					var step = (2*Mathf.PI)/circleResolution;

					for (int i = 0; i < circleResolution; i++) {
						math.sincos(i*step, out float sin, out float cos);
						outputVertices->Add(math.transform(localToGraphMatrix, new float3(cos * radiusX, 0, sin*radiusZ)).xz);
					}

					var y0 = localToGraphMatrix.c3.y;
					outputContours->Add(new NavmeshCut.ContourBurst {
							ymin = y0 - height * 0.5f * scaleFactorY,
							ymax = y0 + height * 0.5f * scaleFactorY,
							startIndex = outputVertices->Length - circleResolution,
							endIndex = outputVertices->Length,
						});
					break;
				}
				case NavmeshCut.MeshType.CustomMesh: {
					if (meshContours != null && meshContourVertices != null && meshScale > 0) {
						localToGraphMatrix = math.mul(localToGraphMatrix, float4x4.Scale(new float3(meshScale)));

						var startIndex = outputVertices->Length;
						for (int i = 0; i < meshContourVertices->Length; i++) {
							outputVertices->Add(math.transform(localToGraphMatrix, meshContourVertices->ElementAt(i)).xz);
						}
						var y0 = localToGraphMatrix.c3.y;
						for (int i = 0; i < meshContours->Length; i++) {
							outputContours->Add(new NavmeshCut.ContourBurst {
									ymin = y0 - this.height * 0.5f * scaleFactorY,
									ymax = y0 + this.height * 0.5f * scaleFactorY,
									startIndex = startIndex + meshContours->ElementAt(i).startIndex,
									endIndex = startIndex + meshContours->ElementAt(i).endIndex,
								});
						}
					}
					break;
				}
				case NavmeshCut.MeshType.Box: {
					// radiusMargin should not be affected by the matrices at all. So we need to compensate for that.
					var boxSize = new float3(rectangleSize.x, height, rectangleSize.y) + math.rcp(new float3(scaleFactorX, scaleFactorY, scaleFactorZ))*radiusMargin*2;

					localToGraphMatrix = math.mul(localToGraphMatrix, float4x4.Scale(boxSize));
					NavmeshCutJobs.BoxConvexHullXZ(localToGraphMatrix, outputVertices, out int numPoints, out float ymin, out float ymax);
					outputContours->Add(new NavmeshCut.ContourBurst {
							ymin = ymin,
							ymax = ymax,
							startIndex = outputVertices->Length - numPoints,
							endIndex = outputVertices->Length,
						});
					break;
				}
				case NavmeshCut.MeshType.Capsule: {
					circleResolution = math.max(circleResolution, 6);

					var r = this.circleRadius;
					// Capsule
					float h = this.height;
					// When scaling along the Y axis we apply this as a height to the capsule instead of just doing a raw scale.
					// This matches what the capsule collider in unity does.
					h *= scaleFactorY;
					localToGraphMatrix = math.mul(localToGraphMatrix, float4x4.Scale(new float3(1.0f, 1.0f/scaleFactorY, 1.0f)));

					NavmeshCutJobs.CapsuleConvexHullXZ(localToGraphMatrix, outputVertices, h, r, radiusMargin, circleResolution, out int numPoints, out float ymin, out float ymax);
					outputContours->Add(new NavmeshCut.ContourBurst {
							ymin = ymin,
							ymax = ymax,
							startIndex = outputVertices->Length - numPoints,
							endIndex = outputVertices->Length,
						});
					break;
				}
				}

				for (int i = 0; i < outputContours->Length; i++) {
					var contour = outputContours->ElementAt(i);
					WindCounterClockwise(outputVertices, contour.startIndex, contour.endIndex);
				}
			}

			/// <summary>Winds the vertices correctly. The particular winding doesn't matter, but all cuts must have the same winding order.</summary>
			private unsafe void WindCounterClockwise (UnsafeList<float2>* vertices, int startIndex, int endIndex) {
				int leftmostIndex = 0;
				float2 leftmost = new float2(float.PositiveInfinity, float.PositiveInfinity);

				for (int i = startIndex; i < endIndex; i++) {
					float2 p = vertices->ElementAt(i);
					if (p.x < leftmost.x || (p.x == leftmost.x && p.y < leftmost.y)) {
						leftmostIndex = i;
						leftmost = p;
					}
				}

				var len = endIndex - startIndex;
				var a = (*vertices)[((leftmostIndex-1 - startIndex + len) % len) + startIndex];
				var b = leftmost;
				var c = (*vertices)[((leftmostIndex+1 - startIndex) % len) + startIndex];

				var clockwise = (b.x-a.x)*(c.y-a.y)-(c.x-a.x)*(b.y-a.y) > 0;
				if (clockwise) {
					// Reverse vertices
					for (int i = startIndex, j = endIndex - 1; i < j; i++, j--) {
						var tmp = vertices->ElementAt(i);
						vertices->ElementAt(i) = vertices->ElementAt(j);
						vertices->ElementAt(j) = tmp;
					}
				}
			}
		}

		/// <summary>
		/// Adjust the radius so that the contour better approximates a circle.
		/// Instead of all points laying exactly on the circle, which means all of the contour is inside the circle,
		/// we change it so that half of the contour is inside and half is outside.
		///
		/// Returns the new radius
		/// </summary>
		static float ApproximateCircleWithPolylineRadius (float radius, int resolution) {
			return radius / (1 - (1 - math.cos(math.PI / resolution)) * 0.5f);
		}

		public static unsafe void CapsuleConvexHullXZ (float4x4 matrix, UnsafeList<float2>* points, float height, float radius, float radiusMargin, int circleResolution, out int numPoints, out float minY, out float maxY) {
			// Calculate the points of the capsule and project them to the XZ plane
			height = math.max(height, radius*2);

			// The contour is split into 2 semicircles.
			var halfRes = circleResolution/2;
			radius = ApproximateCircleWithPolylineRadius(radius, halfRes*2);

			// Figure out the scale factors for the x/z dimensions (width) of the capsule.
			// Pick the largest one in case it is non-uniformly scaled.
			var scaleFactorX = math.length(matrix.c0.xyz);
			var scaleFactorZ = math.length(matrix.c2.xyz);
			radius *= math.max(scaleFactorX, scaleFactorZ);

			var mainAxis = math.normalizesafe(matrix.c1.xyz);
			var start = math.transform(matrix, new float3(0, -height*0.5f, 0)) + mainAxis * radius;
			var end = math.transform(matrix, new float3(0, height*0.5f, 0)) - mainAxis * radius;

			var startXZ = start.xz;
			var endXZ = end.xz;
			float2 axis1;

			bool circle = false;
			if (math.lengthsq(startXZ - endXZ) < 0.005f) {
				// Circle
				axis1 = new float2(1.0f, 0.0f);
				circle = true;
			} else {
				axis1 = math.normalize(endXZ - startXZ);
			}

			var axis2 = new float2(-axis1.y, axis1.x);

			// The additional margin is applied last. This will be in graph space.
			radius += radiusMargin;

			axis1 *= radius;
			axis2 *= radius;

			minY = math.min(start.y, end.y) - radius;
			maxY = math.max(start.y, end.y) + radius;

			var step = math.PI / halfRes;

			if (circle) {
				// Special case the circle to avoid multiple vertices being placed at the same spot
				numPoints = halfRes*2;
				var startIndex = points->Length;
				points->Resize(points->Length + numPoints, NativeArrayOptions.UninitializedMemory);
				for (int i = 0; i < halfRes; i++) {
					var t = i * step;
					math.sincos(t, out float sin, out float cos);
					var dir = sin * axis1 + cos * axis2;
					var p1 = startXZ - dir;
					var p2 = endXZ + dir;
					points->ElementAt(startIndex + i) = p1;
					points->ElementAt(startIndex + i + halfRes) = p2;
				}
			} else {
				// We split into two semicircles.
				// We need to duplicate 2 points for this to work
				numPoints = (halfRes+1)*2;
				var startIndex = points->Length;
				points->Resize(points->Length + numPoints, NativeArrayOptions.UninitializedMemory);
				for (int i = 0; i < halfRes + 1; i++) {
					var t = i * step;
					math.sincos(t, out float sin, out float cos);
					var dir = sin * axis1 + cos * axis2;
					var p1 = startXZ - dir;
					var p2 = endXZ + dir;
					points->ElementAt(startIndex + i) = p1;
					points->ElementAt(startIndex + i + halfRes + 1) = p2;
				}
			}
		}

		public static unsafe void BoxConvexHullXZ (float4x4 matrix, UnsafeList<float2>* points, out int numPoints, out float minY, out float maxY) {
			// Calculate the 8 points of the box and project them to the XZ plane
			minY = float.PositiveInfinity;
			maxY = float.NegativeInfinity;
			var startIndex = points->Length;
			points->Resize(points->Length + BoxCorners.Length, NativeArrayOptions.UninitializedMemory);
			for (int i = 0; i < BoxCorners.Length; i++) {
				var p = math.mul(matrix, BoxCorners[i]);
				minY = math.min(minY, p.y);
				maxY = math.max(maxY, p.y);
				points->ElementAt(startIndex + i) = p.xz;
			}
			numPoints = ConvexHull(points->Ptr + startIndex, BoxCorners.Length, 0.01f);
			// Remove garbage at the end
			points->Length = startIndex + numPoints;
		}

		struct AngleComparator : IComparer<float2> {
			public float2 origin;
			public int Compare (float2 lhs, float2 rhs) {
				// cross product of (lhs - origin) and (rhs - origin)
				var a = lhs - origin;
				var b = rhs - origin;
				var cross = a.x*b.y - a.y*b.x;

				if (Unity.Burst.CompilerServices.Hint.Unlikely(cross == 0)) {
					var la = math.lengthsq(a);
					var lb = math.lengthsq(b);

					if (la < lb) return 1;
					else if (la > lb) return -1;

					// At this point, either lhs == rhs, or lhs == -rhs
					// Sort first by x, then by y
					if (a.x < b.x) return 1;
					else if (a.x > b.x) return -1;

					if (a.y < b.y) return 1;
					else if (a.y > b.y) return -1;

					return 0;
				} else {
					return cross < 0 ? 1 : -1;
				}
			}
		}

		/// <summary>
		/// Calculates the convex hull of a point set using the graham scan algorithm.
		///
		/// The `points` array will be modified to contain the convex hull.
		/// The number of vertices on the hull is returned by this function.
		///
		/// Vertices on the hull closer than `vertexMergeDistance` will be merged together.
		///
		/// From KTH ACM Contest Template Library (2015 version)
		/// </summary>
		public static unsafe int ConvexHull (float2* points, int nPoints, float vertexMergeDistance) {
			// Point with lowest x coordinate. Breaks ties along the y axis.
			var startingIndex = 0;

			for (int i = 0; i < nPoints; i++) {
				if (points[i].x < points[startingIndex].x || (points[i].x == points[startingIndex].x && points[i].y < points[startingIndex].y)) {
					startingIndex = i;
				}
			}

			NativeSortExtension.Sort(points, nPoints, new AngleComparator {
				origin = points[startingIndex]
			});

			var writeIndex = 0;

			for (int i = 0; i < nPoints; i++) {
				var p = points[i];
				while (writeIndex >= 2) {
					var a = points[writeIndex-1] - p;
					var b = points[writeIndex-2] - p;
					var cross = a.x*b.y - a.y*b.x;
					if (cross >= 0 || math.lengthsq(a) < vertexMergeDistance) {
						writeIndex--;
					} else {
						break;
					}
				}
				// Important to make sure 2 identical points at the start don't end up in the output
				if (writeIndex == 1 && math.lengthsq(points[writeIndex-1] - p) < vertexMergeDistance) {
					writeIndex--;
				}

				points[writeIndex] = p;
				writeIndex++;
			}

			return writeIndex;
		}
	}
}