+ astar project

6 files changed, 1400 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/MovementUtilities.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/MovementUtilities.cs
new file mode 100644
index 0000000..cf08a20
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/MovementUtilities.cs
@@ -0,0 +1,208 @@
+using UnityEngine;
+
+namespace Pathfinding.Util {
+	public static class MovementUtilities {
+		public static float FilterRotationDirection (ref Vector2 state, ref Vector2 state2, Vector2 deltaPosition, float threshold, float deltaTime, bool avoidingOtherAgents) {
+			const float Decay = 0.5f;
+
+			var lastState = state;
+
+			if (!avoidingOtherAgents) {
+				// When not avoiding other agents, we can be a bit more aggressive with rotating towards the target.
+				// This is because in that case, the velocity is much less noisy.
+				state += deltaPosition * 10;
+			} else {
+				state += deltaPosition;
+			}
+
+			// Decay the state slowly (has the most effect if the agent is standing still)
+			state *= Mathf.Clamp01(1.0f - deltaTime*Decay);
+			float stateLength = state.magnitude;
+
+			if (stateLength > threshold*2f) {
+				state = state * (threshold*2f/stateLength);
+				stateLength = threshold*2f;
+			}
+
+			// TODO: Figure out what to do with
+			state2 += (state - lastState) * Decay;
+			state2 *= Mathf.Clamp01(1.0f - deltaTime*Decay);
+
+			// Prevent rotation if the agent doesn't move much
+			float speed = stateLength > threshold ? 1.0f : 0.0f;
+			return speed;
+		}
+
+		/// <summary>
+		/// Clamps the velocity to the max speed and optionally the forwards direction.
+		///
+		/// Note that all vectors are 2D vectors, not 3D vectors.
+		///
+		/// Returns: The clamped velocity in world units per second.
+		/// </summary>
+		/// <param name="velocity">Desired velocity of the character. In world units per second.</param>
+		/// <param name="maxSpeed">Max speed of the character. In world units per second.</param>
+		/// <param name="speedLimitFactor">Value between 0 and 1 which determines how much slower the character should move than normal.
+		///      Normally 1 but should go to 0 when the character approaches the end of the path.</param>
+		/// <param name="slowWhenNotFacingTarget">Slow the character down if the desired velocity is not in the same direction as the forward vector.</param>
+		/// <param name="preventMovingBackwards">Prevent the velocity from being too far away from the forward direction of the character.</param>
+		/// <param name="forward">Forward direction of the character. Used together with the slowWhenNotFacingTarget parameter.</param>
+		public static Vector2 ClampVelocity (Vector2 velocity, float maxSpeed, float speedLimitFactor, bool slowWhenNotFacingTarget, bool preventMovingBackwards, Vector2 forward) {
+			// Max speed to use for this frame
+			var currentMaxSpeed = maxSpeed * speedLimitFactor;
+
+			// Check if the agent should slow down in case it is not facing the direction it wants to move in
+			if (slowWhenNotFacingTarget && (forward.x != 0 || forward.y != 0)) {
+				float currentSpeed;
+				var normalizedVelocity = VectorMath.Normalize(velocity, out currentSpeed);
+				float dot = Vector2.Dot(normalizedVelocity, forward);
+
+				// Lower the speed when the character's forward direction is not pointing towards the desired velocity
+				// 1 when velocity is in the same direction as forward
+				// 0.2 when they point in the opposite directions
+				float directionSpeedFactor = Mathf.Clamp(dot+0.707f, 0.2f, 1.0f);
+				currentMaxSpeed *= directionSpeedFactor;
+				currentSpeed = Mathf.Min(currentSpeed, currentMaxSpeed);
+
+				if (preventMovingBackwards) {
+					// Angle between the forwards direction of the character and our desired velocity
+					float angle = Mathf.Acos(Mathf.Clamp(dot, -1, 1));
+
+					// Clamp the angle to 20 degrees
+					// We cannot keep the velocity exactly in the forwards direction of the character
+					// because we use the rotation to determine in which direction to rotate and if
+					// the velocity would always be in the forwards direction of the character then
+					// the character would never rotate.
+					// Allow larger angles when near the end of the path to prevent oscillations.
+					angle = Mathf.Min(angle, (20f + 180f*(1 - speedLimitFactor*speedLimitFactor))*Mathf.Deg2Rad);
+
+					float sin = Mathf.Sin(angle);
+					float cos = Mathf.Cos(angle);
+
+					// Determine if we should rotate clockwise or counter-clockwise to move towards the current velocity
+					sin *= Mathf.Sign(normalizedVelocity.x*forward.y - normalizedVelocity.y*forward.x);
+					// Rotate the #forward vector by #angle radians
+					// The rotation is done using an inlined rotation matrix.
+					// See https://en.wikipedia.org/wiki/Rotation_matrix
+					return new Vector2(forward.x*cos + forward.y*sin, forward.y*cos - forward.x*sin) * currentSpeed;
+				} else {
+					return normalizedVelocity * currentSpeed;
+				}
+			} else {
+				return Vector2.ClampMagnitude(velocity, currentMaxSpeed);
+			}
+		}
+
+		/// <summary>Calculate an acceleration to move deltaPosition units and get there with approximately a velocity of targetVelocity</summary>
+		public static Vector2 CalculateAccelerationToReachPoint (Vector2 deltaPosition, Vector2 targetVelocity, Vector2 currentVelocity, float forwardsAcceleration, float rotationSpeed, float maxSpeed, Vector2 forwardsVector) {
+			// Guard against div by zero
+			if (forwardsAcceleration <= 0) return Vector2.zero;
+
+			float currentSpeed = currentVelocity.magnitude;
+
+			// Convert rotation speed to an acceleration
+			// See https://en.wikipedia.org/wiki/Centripetal_force
+			var sidewaysAcceleration = currentSpeed * rotationSpeed * Mathf.Deg2Rad;
+
+			// To avoid weird behaviour when the rotation speed is very low we allow the agent to accelerate sideways without rotating much
+			// if the rotation speed is very small. Also guards against division by zero.
+			sidewaysAcceleration = Mathf.Max(sidewaysAcceleration, forwardsAcceleration);
+
+			// Transform coordinates to local space where +X is the forwards direction
+			// This is essentially equivalent to Transform.InverseTransformDirection.
+			deltaPosition = VectorMath.ComplexMultiplyConjugate(deltaPosition, forwardsVector);
+			targetVelocity = VectorMath.ComplexMultiplyConjugate(targetVelocity, forwardsVector);
+			currentVelocity = VectorMath.ComplexMultiplyConjugate(currentVelocity, forwardsVector);
+			float ellipseSqrFactorX = 1 / (forwardsAcceleration*forwardsAcceleration);
+			float ellipseSqrFactorY = 1 / (sidewaysAcceleration*sidewaysAcceleration);
+
+			// If the target velocity is zero we can use a more fancy approach
+			// and calculate a nicer path.
+			// In particular, this is the case at the end of the path.
+			if (targetVelocity == Vector2.zero) {
+				// Run a binary search over the time to get to the target point.
+				float mn = 0.01f;
+				float mx = 10;
+				while (mx - mn > 0.01f) {
+					var time = (mx + mn) * 0.5f;
+
+					// Given that we want to move deltaPosition units from out current position, that our current velocity is given
+					// and that when we reach the target we want our velocity to be zero. Also assume that our acceleration will
+					// vary linearly during the slowdown. Then we can calculate what our acceleration should be during this frame.
+
+					//{ t = time
+					//{ deltaPosition = vt + at^2/2 + qt^3/6
+					//{ 0 = v + at + qt^2/2
+					//{ solve for a
+					// a = acceleration vector
+					// q = derivative of the acceleration vector
+					var a = (6*deltaPosition - 4*time*currentVelocity)/(time*time);
+					var q = 6*(time*currentVelocity - 2*deltaPosition)/(time*time*time);
+
+					// Make sure the acceleration is not greater than our maximum allowed acceleration.
+					// If it is we increase the time we want to use to get to the target
+					// and if it is not, we decrease the time to get there faster.
+					// Since the acceleration is described by acceleration = a + q*t
+					// we only need to check at t=0 and t=time.
+					// Note that the acceleration limit is described by an ellipse, not a circle.
+					var nextA = a + q*time;
+					if (a.x*a.x*ellipseSqrFactorX + a.y*a.y*ellipseSqrFactorY > 1.0f || nextA.x*nextA.x*ellipseSqrFactorX + nextA.y*nextA.y*ellipseSqrFactorY > 1.0f) {
+						mn = time;
+					} else {
+						mx = time;
+					}
+				}
+
+				var finalAcceleration = (6*deltaPosition - 4*mx*currentVelocity)/(mx*mx);
+
+				// Boosting
+				{
+					// The trajectory calculated above has a tendency to use very wide arcs
+					// and that does unfortunately not look particularly good in some cases.
+					// Here we amplify the component of the acceleration that is perpendicular
+					// to our current velocity. This will make the agent turn towards the
+					// target quicker.
+					// How much amplification to use. Value is unitless.
+					const float Boost = 1;
+					finalAcceleration.y *= 1 + Boost;
+
+					// Clamp the velocity to the maximum acceleration.
+					// Note that the maximum acceleration constraint is shaped like an ellipse, not like a circle.
+					float ellipseMagnitude = finalAcceleration.x*finalAcceleration.x*ellipseSqrFactorX + finalAcceleration.y*finalAcceleration.y*ellipseSqrFactorY;
+					if (ellipseMagnitude > 1.0f) finalAcceleration /= Mathf.Sqrt(ellipseMagnitude);
+				}
+
+				return VectorMath.ComplexMultiply(finalAcceleration, forwardsVector);
+			} else {
+				// Here we try to move towards the next waypoint which has been modified slightly using our
+				// desired velocity at that point so that the agent will more smoothly round the corner.
+
+				// How much to strive for making sure we reach the target point with the target velocity. Unitless.
+				const float TargetVelocityWeight = 0.5f;
+
+				// Limit to how much to care about the target velocity. Value is in seconds.
+				// This prevents the character from moving away from the path too much when the target point is far away
+				const float TargetVelocityWeightLimit = 1.5f;
+				float targetSpeed;
+				var normalizedTargetVelocity = VectorMath.Normalize(targetVelocity, out targetSpeed);
+
+				var distance = deltaPosition.magnitude;
+				var targetPoint = deltaPosition - normalizedTargetVelocity * System.Math.Min(TargetVelocityWeight * distance * targetSpeed / (currentSpeed + targetSpeed), maxSpeed*TargetVelocityWeightLimit);
+
+				// How quickly the agent will try to reach the velocity that we want it to have.
+				// We need this to prevent oscillations and jitter which is what happens if
+				// we let the constant go towards zero. Value is in seconds.
+				const float TimeToReachDesiredVelocity = 0.1f;
+				// TODO: Clamp to ellipse using more accurate acceleration (use rotation speed as well)
+				var finalAcceleration = (targetPoint.normalized*maxSpeed - currentVelocity) * (1f/TimeToReachDesiredVelocity);
+
+				// Clamp the velocity to the maximum acceleration.
+				// Note that the maximum acceleration constraint is shaped like an ellipse, not like a circle.
+				float ellipseMagnitude = finalAcceleration.x*finalAcceleration.x*ellipseSqrFactorX + finalAcceleration.y*finalAcceleration.y*ellipseSqrFactorY;
+				if (ellipseMagnitude > 1.0f) finalAcceleration /= Mathf.Sqrt(ellipseMagnitude);
+
+				return VectorMath.ComplexMultiply(finalAcceleration, forwardsVector);
+			}
+		}
+	}
+}
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/MovementUtilities.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/MovementUtilities.cs.meta
new file mode 100644
index 0000000..bb4e60d
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/MovementUtilities.cs.meta
@@ -0,0 +1,12 @@
+fileFormatVersion: 2
+guid: 0a6cffd9895f94907aa43f18b0904587
+timeCreated: 1490097740
+licenseType: Store
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDMovement.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDMovement.cs
new file mode 100644
index 0000000..3dd76dd
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDMovement.cs
@@ -0,0 +1,1110 @@
+using UnityEngine;
+using Unity.Mathematics;
+using System.Collections.Generic;
+using Unity.Collections;
+using UnityEngine.Profiling;
+
+namespace Pathfinding.PID {
+	using Pathfinding.Drawing;
+	using Pathfinding.Util;
+	using Palette = Pathfinding.Drawing.Palette.Colorbrewer.Set1;
+	using Unity.Jobs;
+	using Unity.Profiling;
+	using UnityEngine.Assertions;
+	using Unity.Burst;
+	using Unity.Collections.LowLevel.Unsafe;
+	using Pathfinding.RVO;
+
+	/// <summary>Core control loop for the <see cref="FollowerEntity"/> movement script</summary>
+	[System.Serializable]
+	[BurstCompile]
+	public struct PIDMovement {
+		public struct PersistentState {
+			public float maxDesiredWallDistance;
+		}
+
+		/// <summary>
+		/// Desired rotation speed in degrees per second.
+		///
+		/// If the agent is in an open area and gets a new destination directly behind itself, it will start to rotate around with exactly this rotation speed.
+		///
+		/// The agent will slow down its rotation speed as it approaches its desired facing direction.
+		/// So for example, when it is only 90 degrees away from its desired facing direction, it will only rotate with about half this speed.
+		///
+		/// See: <see cref="maxRotationSpeed"/>
+		/// </summary>
+		public float rotationSpeed;
+
+		/// <summary>
+		/// Desired speed of the agent in meters per second.
+		///
+		/// This will be multiplied by the agent's scale to get the actual speed.
+		/// </summary>
+		public float speed;
+
+		/// <summary>
+		/// Maximum rotation speed in degrees per second.
+		///
+		/// If the agent would have to rotate faster than this, it will instead slow down to get more time to rotate.
+		///
+		/// The agent may want to rotate faster than <see cref="rotationSpeed"/> if there's not enough space, so that it has to move in a more narrow arc.
+		/// It may also want to rotate faster if it is very close to its destination and it wants to make sure it ends up on the right spot without any circling.
+		///
+		/// It is recommended to keep this at a value slightly greater than <see cref="rotationSpeed"/>.
+		///
+		/// See: <see cref="rotationSpeed"/>
+		/// </summary>
+		public float maxRotationSpeed;
+
+		/// <summary>
+		/// Maximum rotation speed in degrees per second while rotating on the spot.
+		///
+		/// Only used if <see cref="allowRotatingOnSpot"/> is enabled.
+		/// </summary>
+		public float maxOnSpotRotationSpeed;
+
+		/// <summary>
+		/// Time for the agent to slow down to a complete stop when it approaches the destination point, in seconds.
+		///
+		/// One can calculate the deceleration like: <see cref="speed"/>/<see cref="slowdownTime"/> (with units m/s^2).
+		/// </summary>
+		public float slowdownTime;
+
+		/// <summary>
+		/// Time for the agent to slow down to a complete stop when it decides to change direction by turning on the spot
+		///
+		/// If set to zero, the agent will instantly stop and start to turn around.
+		///
+		/// Only used if <see cref="allowRotatingOnSpot"/> is enabled.
+		/// </summary>
+		public float slowdownTimeWhenTurningOnSpot;
+
+		/// <summary>
+		/// How big of a distance to try to keep from obstacles.
+		///
+		/// Typically around 1 or 2 times the agent radius is a good value for this.
+		///
+		/// Try to avoid making it so large that there might not be enough space for the agent to keep this amount of distance from obstacles.
+		/// It may start to move less optimally if it is not possible to keep this distance.
+		///
+		/// This works well in open spaces, but if your game consists of a lot of tight corridors, a low, or zero value may be better.
+		///
+		/// This will be multiplied by the agent's scale to get the actual distance.
+		/// </summary>
+		public float desiredWallDistance;
+
+		/// <summary>
+		/// How wide of a turn to make when approaching a destination for which a desired facing direction has been set.
+		///
+		/// The following video shows three agents, one with no facing direction set, and then two agents with varying values of the lead in radius.
+		/// [Open online documentation to see videos]
+		///
+		/// Setting this to zero will make the agent move directly to the end of the path and rotate on the spot to face the desired facing direction, once it is there.
+		///
+		/// When approaching a destination for which no desired facing direction has been set, this field has no effect.
+		///
+		/// Warning: Setting this to a too small (but non-zero) value may look bad if the agent cannot rotate fast enough to stay on the arc.
+		///
+		/// This will be multiplied by the agent's scale to get the actual radius.
+		/// </summary>
+		public float leadInRadiusWhenApproachingDestination;
+
+		/// <summary>If rotation on the spot is allowed or not</summary>
+		public bool allowRotatingOnSpot {
+			get => allowRotatingOnSpotBacking != 0;
+			set => allowRotatingOnSpotBacking = (byte)(value ? 1 : 0);
+		}
+
+		/// <summary>
+		/// If rotation on the spot is allowed or not.
+		/// 1 for allowed, 0 for not allowed.
+		///
+		/// That we have to use a byte instead of a boolean is due to a Burst limitation.
+		/// </summary>
+		[SerializeField]
+		byte allowRotatingOnSpotBacking;
+
+		public const float DESTINATION_CLEARANCE_FACTOR = 4f;
+
+		private static readonly ProfilerMarker MarkerSidewaysAvoidance = new ProfilerMarker("SidewaysAvoidance");
+		private static readonly ProfilerMarker MarkerPID = new ProfilerMarker("PID");
+		private static readonly ProfilerMarker MarkerOptimizeDirection = new ProfilerMarker("OptimizeDirection");
+		private static readonly ProfilerMarker MarkerSmallestDistance = new ProfilerMarker("ClosestDistance");
+		private static readonly ProfilerMarker MarkerConvertObstacles = new ProfilerMarker("ConvertObstacles");
+
+		[System.Flags]
+		public enum DebugFlags {
+			Nothing = 0,
+			Position = 1 << 0,
+			Tangent = 1 << 1,
+			SidewaysClearance = 1 << 2,
+			ForwardClearance = 1 << 3,
+			Obstacles = 1 << 4,
+			Funnel = 1 << 5,
+			Path = 1 << 6,
+			ApproachWithOrientation = 1 << 7,
+			Rotation = 1 << 8,
+		}
+
+		public void ScaleByAgentScale (float agentScale) {
+			speed *= agentScale;
+			leadInRadiusWhenApproachingDestination *= agentScale;
+			desiredWallDistance *= agentScale;
+		}
+
+		public float Speed (float remainingDistance) {
+			if (speed <= 0) return 0;
+			if (this.slowdownTime <= 0) return remainingDistance <= 0.0001f ? 0 : speed;
+
+			// This is what you get if you apply a constant deceleration per unit of time of this.speed/this.slowdownTime
+			float slowdownFactor = Mathf.Min(1.0f, Mathf.Sqrt(2 * remainingDistance / (speed * this.slowdownTime)));
+
+			var res = speed * slowdownFactor;
+			Assert.IsTrue(math.isfinite(res));
+			return res;
+		}
+
+		/// <summary>
+		/// Accelerates as quickly as possible.
+		///
+		/// This follows the same curve as the <see cref="Speed"/> function, as a function of the remaining distance.
+		///
+		/// Returns: The speed the agent should have after accelerating for dt seconds. Assuming dt is small.
+		/// </summary>
+		/// <param name="speed">The current speed of the agent.</param>
+		/// <param name="timeToReachMaxSpeed">The time it takes for the agent to reach the maximum speed, starting from a standstill.</param>
+		/// <param name="dt">The time to accelerate for. Can be negative to decelerate instead.</param>
+		public float Accelerate (float speed, float timeToReachMaxSpeed, float dt) {
+			// This can be derived by assuming a constant deceleration per unit of time:
+			// x''(t) = A
+			// Integrating twice gives us
+			// x'(t) = A * t
+			// x(t) = A * t^2 / 2
+			//
+			// Ensuring it yields the same output as the Speed function gives us the equation:
+			// x'(t) = S * sqrt(2 * x(t) / (S * T))
+			// A * t = S * sqrt(2 * (A * t^2 / 2) / (S * T))
+			//
+			// Which yields the acceleration when solved:
+			// A = S / T
+			if (timeToReachMaxSpeed > 0.001f) {
+				var a = this.speed / timeToReachMaxSpeed;
+				return math.clamp(speed + dt * a, 0, this.speed);
+			} else {
+				return dt > 0 ? this.speed : 0;
+			}
+		}
+
+		public float CurveFollowingStrength (float signedDistToClearArea, float radiusToWall, float remainingDistance) {
+			var speed = math.max(0.00001f, this.speed);
+			var followingStrength = AnglePIDController.RotationSpeedToFollowingStrength(speed, math.radians(this.rotationSpeed));
+			var modifiedAlpha = math.max(followingStrength, 40.0f * math.pow(math.abs(signedDistToClearArea) / math.max(0.0001f, radiusToWall), 1));
+			var remainingTime = remainingDistance / speed;
+
+			// Just before reaching the end of the path, the agent should try to follow the path very closely to avoid overshooting,
+			// and potentially spinning in place.
+			const float HIGH_EFFORT_TIME = 0.2f;
+			modifiedAlpha = math.max(modifiedAlpha, math.min(80.0f, math.pow(1.0f / math.max(0, remainingTime - HIGH_EFFORT_TIME), 3)));
+
+			Assert.IsTrue(math.isfinite(modifiedAlpha));
+			return modifiedAlpha;
+		}
+
+		static bool ClipLineByHalfPlaneX (ref float2 a, ref float2 b, float x, float side) {
+			var wrongSideA = (a.x - x)*side < 0;
+			var wrongSideB = (b.x - x)*side < 0;
+			if (wrongSideA && wrongSideB) return false;
+			if (wrongSideA != wrongSideB) {
+				var t = math.unlerp(a.x, b.x, x);
+				var intersection = math.lerp(a, b, t);
+				if (wrongSideA) a = intersection;
+				else b = intersection;
+			}
+			return true;
+		}
+
+		static void ClipLineByHalfPlaneYt (float2 a, float2 b, float y, float side, ref float mnT, ref float mxT) {
+			var wrongSideA = (a.y - y)*side < 0;
+			var wrongSideB = (b.y - y)*side < 0;
+			if (wrongSideA && wrongSideB) {
+				mnT = 1;
+				mxT = 0;
+			} else if (wrongSideA != wrongSideB) {
+				var t = math.unlerp(a.y, b.y, y);
+				if (wrongSideA) mnT = math.max(mnT, t);
+				else mxT = math.min(mxT, t);
+			}
+		}
+
+		/// <summary>
+		/// Returns either the most clockwise, or most counter-clockwise direction of the three given directions.
+		/// The directions are compared pairwise, not using any global reference angle.
+		/// </summary>
+		static float2 MaxAngle (float2 a, float2 b, float2 c, bool clockwise) {
+			a = math.select(a, b, VectorMath.Determinant(a, b) < 0 == clockwise);
+			a = math.select(a, c, VectorMath.Determinant(a, c) < 0 == clockwise);
+			return a;
+		}
+
+		/// <summary>
+		/// Returns either the most clockwise, or most counter-clockwise direction of the two given directions.
+		/// The directions are compared pairwise, not using any global reference angle.
+		/// </summary>
+		static float2 MaxAngle (float2 a, float2 b, bool clockwise) {
+			return math.select(a, b, VectorMath.Determinant(a, b) < 0 == clockwise);
+		}
+
+		const float ALLOWED_OVERLAP_FACTOR = 0.1f;
+		const float STEP_MULTIPLIER = 1.0f;
+		const float MAX_FRACTION_OF_REMAINING_DISTANCE = 0.9f;
+		const int OPTIMIZATION_ITERATIONS = 8;
+
+		static void DrawChisel (float2 start, float2 direction, float pointiness, float length, float width, CommandBuilder draw, Color col) {
+			draw.PushColor(col);
+			var cornerL = start + (direction * pointiness + new float2(-direction.y, direction.x)) * width;
+			var cornerR = start + (direction * pointiness - new float2(-direction.y, direction.x)) * width;
+			draw.xz.Line(start, cornerL, col);
+			draw.xz.Line(start, cornerR, col);
+			var remainingLength = length - pointiness * width;
+			if (remainingLength > 0) {
+				draw.xz.Ray(cornerL, direction * remainingLength, col);
+				draw.xz.Ray(cornerR, direction * remainingLength, col);
+			}
+			draw.PopColor();
+		}
+
+		static void SplitSegment (float2 e1, float2 e2, float desiredRadius, float length, float pointiness, ref EdgeBuffers buffers) {
+			// Check if it is completely outside the range we concern ourselves with.
+			// When the direction is rotated, we may end up caring about segments further to the side than #desiredRadius, but with
+			// a safety margin of 2, we should catch all potential segments that we care about.
+			float radiusWithMargin = desiredRadius * 2f;
+			if ((e1.y < -radiusWithMargin && e2.y < -radiusWithMargin) || (e1.y > radiusWithMargin && e2.y > radiusWithMargin)) return;
+
+			// Remove the part of the segment that is behind the agent
+			if (!ClipLineByHalfPlaneX(ref e1, ref e2, 0, 1)) return;
+
+			// We don't care about any segments further away than #length
+			if (!VectorMath.SegmentCircleIntersectionFactors(e1, e2, length*length, out var t1, out var t2)) {
+				// Completely outside the circle
+				return;
+			}
+
+			// Remove the parts of segments that are really close to the agent.
+			// Otherwise it can try to aggressively avoid segments that are super close, but are only obstacles due to minimal floating point errors.
+			var thresholdRadius = desiredRadius*0.01f;
+			if (VectorMath.SegmentCircleIntersectionFactors(e1, e2, thresholdRadius*thresholdRadius, out var tInner1, out var tInner2) && tInner1 < t2 && tInner2 > t1) {
+				// Remove the intersection with the inner circle.
+				// This may split the segment into 0, 1 or 2 parts.
+				if (tInner1 > t1 && tInner1 < t2) SplitSegment2(math.lerp(e1, e2, t1), math.lerp(e1, e2, tInner1), desiredRadius, pointiness, ref buffers);
+				if (tInner2 > t1 && tInner2 < t2) SplitSegment2(math.lerp(e1, e2, tInner2), math.lerp(e1, e2, t2), desiredRadius, pointiness, ref buffers);
+			} else {
+				// No intersection with the inner circle. This is the common case.
+				SplitSegment2(math.lerp(e1, e2, t1), math.lerp(e1, e2, t2), desiredRadius, pointiness, ref buffers);
+			}
+		}
+
+		static void SplitSegment2 (float2 e1, float2 e2, float desiredRadius, float pointiness, ref EdgeBuffers buffers) {
+			// The shape that we use for avoidance looks like this:
+			//    __________
+			//   /
+			//  /
+			//  \
+			//   \ __________
+			//
+			// With the agent at the pointy end of the shape.
+			// Here we check if the segment overlaps the triangular part of the shape, as defined by a circle with the same radius
+			// as the sides of the triangle.
+			if (VectorMath.SegmentCircleIntersectionFactors(e1, e2, (pointiness*pointiness + 1)*desiredRadius*desiredRadius, out var t1, out var t2)) {
+				// Split the segment at the intersection with the circle
+				// This may split the segment into 0, 1, 2 or 3 parts.
+				if (t1 > 0.0f && t2 < 1.0f) {
+					SplitSegment3(e1, math.lerp(e1, e2, t1), desiredRadius, false, ref buffers);
+					SplitSegment3(math.lerp(e1, e2, t1), math.lerp(e1, e2, t2), desiredRadius, true, ref buffers);
+					SplitSegment3(math.lerp(e1, e2, t2), e2, desiredRadius, false, ref buffers);
+				} else if (t1 > 0.0f) {
+					SplitSegment3(e1, math.lerp(e1, e2, t1), desiredRadius, false, ref buffers);
+					SplitSegment3(math.lerp(e1, e2, t1), e2, desiredRadius, true, ref buffers);
+				} else if (t2 < 1.0f) {
+					SplitSegment3(e1, math.lerp(e1, e2, t2), desiredRadius, true, ref buffers);
+					SplitSegment3(math.lerp(e1, e2, t2), e2, desiredRadius, false, ref buffers);
+				} else {
+					// Whole segment
+					SplitSegment3(e1, e2, desiredRadius, true, ref buffers);
+				}
+			} else {
+				// Outside the circle
+				SplitSegment3(e1, e2, desiredRadius, false, ref buffers);
+			}
+		}
+
+		static void SplitSegment3 (float2 e1, float2 e2, float desiredRadius, bool inTriangularRegion, ref EdgeBuffers buffers) {
+			// Check the orientation of the segment, and bias it so that the agent is
+			// more like to try to pass on the "correct" side of the segment.
+			// All obstacle edges that we get from the navmesh are oriented.
+			// However, we only bias the segment when we calculate which side the segment is on,
+			// and when calculating the intersection factor. After that, we return to using
+			// the original segment.
+			//
+			// If we don't do this, then the agent can get stuck at the border of the navmesh.
+			// Even if the agent is clamped to the navmesh, it may end up a tiiny bit outside it,
+			// and then it would try to avoid the border of the navmesh by moving even further outside it.
+			var r1 = e1;
+			var r2 = e2;
+			if (r2.x < r1.x) {
+				r1.y -= 0.01f;
+				r2.y -= 0.01f;
+			} else {
+				r1.y += 0.01f;
+				r2.y += 0.01f;
+			}
+
+			var e1Left = r1.y > 0;
+
+			// Ensure e1 is to the left of the midpoint line
+			if (!e1Left) {
+				Memory.Swap(ref e1, ref e2);
+				Memory.Swap(ref r1, ref r2);
+			}
+
+			// Intersection of the line e1 -> e2 with the line y=0
+			var tIntersection = math.unlerp(r1.y, r2.y, 0f);
+
+			var anyIntersection = math.isfinite(tIntersection);
+			if (tIntersection <= 0.0f || tIntersection >= 1.0f || !anyIntersection) {
+				// No intersection
+				SplitSegment4(e1, e2, inTriangularRegion, e1Left, ref buffers);
+			} else {
+				// Intersection. Split the segment into two parts, one for the left side, and one for the right side.
+				var intersection = e1 + tIntersection * (e2 - e1);
+				var l1 = math.lengthsq(e1 - intersection);
+				var l2 = math.lengthsq(e2 - intersection);
+				var allowedLineOverlap = desiredRadius * ALLOWED_OVERLAP_FACTOR;
+				float allowedLineOverlapSq = allowedLineOverlap*allowedLineOverlap;
+
+				// Check both the left and right subsegments. Ignore them if they are really short.
+				if (l1 > allowedLineOverlapSq || l1 >= l2) SplitSegment4(e1, intersection, inTriangularRegion, true, ref buffers);
+				if (l2 > allowedLineOverlapSq || l2 >= l1) SplitSegment4(intersection, e2, inTriangularRegion, false, ref buffers);
+			}
+		}
+
+		static void SplitSegment4 (float2 e1, float2 e2, bool inTriangularRegion, bool left, ref EdgeBuffers buffers) {
+			// Ignore tiiiny edges
+			// Not quite sure when they get generated, but they do exist.
+			// Including these can cause issues if end up almost, but not quite, on the midpoint line,
+			// near the end.
+			// Ideally we'd have some better code for tolerating cases when there's only a tiny obstacle on the left/right side.
+			if (math.all(math.abs(e1 - e2) < 0.01f)) return;
+
+			ref var buffer = ref buffers.triangleRegionEdgesL;
+			if (inTriangularRegion) {
+				if (left) {} // NOOP
+				else buffer = ref buffers.triangleRegionEdgesR;
+			} else {
+				if (left) buffer = ref buffers.straightRegionEdgesL;
+				else buffer = ref buffers.straightRegionEdgesR;
+			}
+
+			if (buffer.Length + 2 > buffer.Capacity) return;
+			buffer.AddNoResize(e1);
+			buffer.AddNoResize(e2);
+		}
+
+		struct EdgeBuffers {
+			public FixedList512Bytes<float2> triangleRegionEdgesL;
+			public FixedList512Bytes<float2> triangleRegionEdgesR;
+			public FixedList512Bytes<float2> straightRegionEdgesL;
+			public FixedList512Bytes<float2> straightRegionEdgesR;
+		}
+
+		/// <summary>
+		/// Finds a direction to move in that is as close as possible to the desired direction while being clear of obstacles, if possible.
+		/// This keeps the agent from moving too close to walls.
+		/// </summary>
+		/// <param name="start">Current position of the agent.</param>
+		/// <param name="end">Point the agent is moving towards.</param>
+		/// <param name="desiredRadius">The distance the agent should try to keep from obstacles.</param>
+		/// <param name="remainingDistance">Remaining distance in the path.</param>
+		/// <param name="pointiness">Essentially controls how much the agent will cut corners. A higher value will lead to a smoother path,
+		///        but it will also lead to the agent not staying as far away from corners as the desired wall distance parameter would suggest.
+		///        It is a unitless quantity.</param>
+		/// <param name="edges">Edges of obstacles. Each edge is represented by two points.</param>
+		/// <param name="draw">CommandBuilder to use for drawing debug information.</param>
+		/// <param name="debugFlags">Flags to control what debug information to draw.</param>
+		public static float2 OptimizeDirection (float2 start, float2 end, float desiredRadius, float remainingDistance, float pointiness, NativeArray<float2> edges, CommandBuilder draw, DebugFlags debugFlags) {
+			var length = math.length(end - start);
+			var direction0 = math.normalizesafe(end - start);
+			length *= 0.999f;
+			length = math.min(MAX_FRACTION_OF_REMAINING_DISTANCE * remainingDistance, length);
+			if (desiredRadius <= 0.0001f) return direction0;
+
+			var lengthOrig = length;
+			var lengthInvOrig = 1 / lengthOrig;
+
+			// Pre-process all edges by splitting them up and grouping them by zone.
+			// We have 4 zones that we care about:
+			// 1. Within the triangular region near the agent, on the left side of the line from #start to #end
+			// 2. Within the triangular region near the agent, on the right side
+			// 3. Outside the triangular region, on the left side
+			// 4. Outside the triangular region, on the right side
+			// We assume that about 32 edges for each zone is enough. If we find more edges, the remainder will be discarded.
+			// Usually there are only a few edges, so this is not a problem in practice.
+			var buffers = new EdgeBuffers();
+			for (int i = 0; i < edges.Length; i += 2) {
+				// Rotate the edge so that the x-axis corresponds to #direction0
+				var e1 = VectorMath.ComplexMultiplyConjugate(edges[i] - start, direction0);
+				var e2 = VectorMath.ComplexMultiplyConjugate(edges[i+1] - start, direction0);
+				SplitSegment(e1, e2, desiredRadius, length, pointiness, ref buffers);
+			}
+
+			// if ((debugFlags & DebugFlags.ForwardClearance) != 0) {
+			// 	for (int i = 0; i < buffers.straightRegionEdgesL.Length; i += 2) {
+			// 		draw.xz.Line(start + VectorMath.ComplexMultiply(buffers.straightRegionEdgesL[i], direction0), start + VectorMath.ComplexMultiply(buffers.straightRegionEdgesL[i+1], direction0), Palette.Orange);
+			// 	}
+			// 	for (int i = 0; i < buffers.straightRegionEdgesR.Length; i += 2) {
+			// 		draw.xz.Line(start + VectorMath.ComplexMultiply(buffers.straightRegionEdgesR[i], direction0), start + VectorMath.ComplexMultiply(buffers.straightRegionEdgesR[i+1], direction0), Palette.Red);
+			// 	}
+			// 	for (int i = 0; i < buffers.triangleRegionEdgesL.Length; i += 2) {
+			// 		draw.xz.Line(start + VectorMath.ComplexMultiply(buffers.triangleRegionEdgesL[i], direction0), start + VectorMath.ComplexMultiply(buffers.triangleRegionEdgesL[i+1], direction0), Palette.Pink);
+			// 	}
+			// 	for (int i = 0; i < buffers.triangleRegionEdgesR.Length; i += 2) {
+			// 		draw.xz.Line(start + VectorMath.ComplexMultiply(buffers.triangleRegionEdgesR[i], direction0), start + VectorMath.ComplexMultiply(buffers.triangleRegionEdgesR[i+1], direction0), Palette.Purple);
+			// 	}
+			// }
+
+			// Complex number representing how much to rotate the original direction by.
+			// The number (1,0) indicates no rotation.
+			var direction = new float2(1, 0);
+
+			// The optimization usually converges very quickly. Error is approximately O(0.5^n)
+			for (int it = 0; it < OPTIMIZATION_ITERATIONS; it++) {
+				if ((debugFlags & DebugFlags.ForwardClearance) != 0) {
+					var col = Palette.Blue;
+					col.a = 0.5f;
+					var d = VectorMath.ComplexMultiply(direction, direction0);
+					DrawChisel(start, d, pointiness, length, desiredRadius, draw, col);
+					draw.xz.Ray(start, d * length, Palette.Purple);
+					draw.xz.Circle(start, remainingDistance, col);
+				}
+
+				var leftReference = new float2(0, desiredRadius);
+				var rightReference = new float2(0, -desiredRadius);
+				var leftObstacleDir = new float2(length, 0);
+				var rightObstacleDir = new float2(length, 0);
+
+				// Iterate through all edges and calculate how much we need to rotate the direction to avoid them.
+				// We store all directions as complex numbers.
+				for (int i = 0; i < buffers.straightRegionEdgesL.Length; i += 2) {
+					// Rotate the edge so that the x-axis corresponds to #direction
+					var e1 = VectorMath.ComplexMultiplyConjugate(buffers.straightRegionEdgesL[i], direction);
+					var e2 = VectorMath.ComplexMultiplyConjugate(buffers.straightRegionEdgesL[i+1], direction);
+					leftObstacleDir = MaxAngle(leftObstacleDir, e1 - leftReference, e2 - leftReference, true);
+				}
+
+				for (int i = 0; i < buffers.straightRegionEdgesR.Length; i += 2) {
+					var e1 = VectorMath.ComplexMultiplyConjugate(buffers.straightRegionEdgesR[i], direction);
+					var e2 = VectorMath.ComplexMultiplyConjugate(buffers.straightRegionEdgesR[i+1], direction);
+					rightObstacleDir = MaxAngle(rightObstacleDir, e1 - rightReference, e2 - rightReference, false);
+				}
+
+				var referenceDiagonalL = math.normalizesafe(VectorMath.ComplexMultiply(new float2(pointiness*desiredRadius, desiredRadius), direction));
+				var referenceDiagonalR = math.normalizesafe(VectorMath.ComplexMultiply(new float2(pointiness*desiredRadius, -desiredRadius), direction));
+				for (int i = 0; i < buffers.triangleRegionEdgesL.Length; i += 2) {
+					// Rotate the edge so that the x-axis corresponds to #referenceDiagonalL
+					var offset1 = VectorMath.ComplexMultiplyConjugate(buffers.triangleRegionEdgesL[i], referenceDiagonalL);
+					var offset2 = VectorMath.ComplexMultiplyConjugate(buffers.triangleRegionEdgesL[i+1], referenceDiagonalL);
+					var offset = offset2.y < offset1.y ? offset2 : offset1;
+					if (offset.y < 0) leftObstacleDir = MaxAngle(leftObstacleDir, offset, true);
+				}
+
+				for (int i = 0; i < buffers.triangleRegionEdgesR.Length; i += 2) {
+					var offset1 = VectorMath.ComplexMultiplyConjugate(buffers.triangleRegionEdgesR[i], referenceDiagonalR);
+					var offset2 = VectorMath.ComplexMultiplyConjugate(buffers.triangleRegionEdgesR[i+1], referenceDiagonalR);
+					var offset = offset2.y > offset1.y ? offset2 : offset1;
+					if (offset.y > 0) rightObstacleDir = MaxAngle(rightObstacleDir, offset, false);
+				}
+
+				// Do some kind of weighted average of the two directions.
+				// Here we map the length of the obstacle directions as 0=>0 and L=>infinity (but we clamp it to a finite but large value).
+				// Basically we want to give more weight to obstacles closer to the agent.
+				var leftInverseWeight = 1 / math.max(0.000001f, lengthOrig - leftObstacleDir.x*leftObstacleDir.x) - lengthInvOrig;
+				var rightInverseWeight = 1 / math.max(0.000001f, lengthOrig - rightObstacleDir.x*rightObstacleDir.x) - lengthInvOrig;
+				var rTot = math.normalizesafe(leftObstacleDir * rightInverseWeight + rightObstacleDir * leftInverseWeight);
+
+				// Alternative averaging which only takes the sum of the angles
+				// var rTot2 = math.normalizesafe(VectorMath.ComplexMultiply(leftObstacleDir, rightObstacleDir));
+
+				// Approximately multiplying the angle by STEP_MULTIPLIER
+				var rStep = math.lerp(new float2(1, 0), rTot, STEP_MULTIPLIER);
+				direction = math.normalizesafe(VectorMath.ComplexMultiply(direction, rStep));
+				if (leftObstacleDir.y == 0 && rightObstacleDir.y == 0) {
+					// Apparently there were NO obstacles.
+					// We can afford to increase our length check a little bit.
+					// This is important in case we encounter a corner which is on a very pointy obstacle.
+					//             _______
+					//   _ _ _ _  /
+					// A _ _ _ _ <______
+					//
+					// Where A is the agent, trying to move towards the corner marked with a '<'.
+					// In that case, we will find no edges to avoid and we will end up moving directly towards the corner instead
+					// of staying slightly away from the walls. Unless we increase the length check a little bit, that is.
+					//
+					// However, we don't want to increase the length more than the remaining distance to the target minus a small margin,
+					// as that can cause weird movement when approaching a target near a wall. It would try to unnecessarily avoid the wall
+					// causing ocillating movement.
+					length = math.min(remainingDistance * MAX_FRACTION_OF_REMAINING_DISTANCE, math.min(length * 1.1f, lengthOrig * 1.2f));
+				} else {
+					// Decrease the length a bit, to bias the optimization towards closer obstacles
+					length = math.min(length, math.max(desiredRadius * 2.0f, math.min(leftObstacleDir.x, rightObstacleDir.x) * 2.0f));
+				}
+			}
+
+			direction = VectorMath.ComplexMultiply(direction, direction0);
+
+			if ((debugFlags & DebugFlags.ForwardClearance) != 0) {
+				DrawChisel(start, direction, pointiness, length, desiredRadius, draw, Color.black);
+			}
+
+			Assert.IsTrue(!math.any(math.isnan(direction)));
+			return direction;
+		}
+
+		/// <summary>
+		/// Calculates the closest point on any point of an edge that is inside a wedge.
+		///
+		/// Returns: The distance to the closest point on any edge that is inside the wedge.
+		/// </summary>
+		/// <param name="point">The origin point of the wedge (the pointy end).</param>
+		/// <param name="dir1">The first direction of the wedge.</param>
+		/// <param name="dir2">The second direction of the wedge.</param>
+		/// <param name="shrinkAmount">The wedge is shrunk by this amount. In the same units as the input points.</param>
+		/// <param name="edges">The edges to check for intersection with.</param>
+		public static float SmallestDistanceWithinWedge (float2 point, float2 dir1, float2 dir2, float shrinkAmount, NativeArray<float2> edges) {
+			dir1 = math.normalizesafe(dir1);
+			dir2 = math.normalizesafe(dir2);
+
+			// Early out in case the wedge is very narrow.
+			// This is primarily a performance optimization.
+			// If the agent is almost facing the correct direction, then it shouldn't be heading towards an obstacle.
+			const float MIN_ANGLE_COS = 0.999f;
+			if (math.dot(dir1, dir2) > MIN_ANGLE_COS) return float.PositiveInfinity;
+
+			var side = math.sign(VectorMath.Determinant(dir1, dir2));
+			shrinkAmount *= side;
+
+			var closestDistanceSq = float.PositiveInfinity;
+			for (int i = 0; i < edges.Length; i += 2) {
+				var e1 = edges[i] - point;
+				var e2 = edges[i+1] - point;
+
+				// Clip the line by the two half planes that the wedge consists of
+				var e1a = VectorMath.ComplexMultiplyConjugate(e1, dir1);
+				var e2a = VectorMath.ComplexMultiplyConjugate(e2, dir1);
+				var e1b = VectorMath.ComplexMultiplyConjugate(e1, dir2);
+				var e2b = VectorMath.ComplexMultiplyConjugate(e2, dir2);
+				var mnT = 0f;
+				var mxT = 1f;
+
+				ClipLineByHalfPlaneYt(e1a, e2a, shrinkAmount, side, ref mnT, ref mxT);
+				if (mnT > mxT) continue;
+				ClipLineByHalfPlaneYt(e1b, e2b, -shrinkAmount, -side, ref mnT, ref mxT);
+				if (mnT > mxT) continue;
+
+				// Find the distance to the closest point on the clipped line segment
+				var lengthsq = math.lengthsq(e2 - e1);
+				var t = math.clamp(math.dot(e1, e1 - e2) * math.rcp(lengthsq), mnT, mxT);
+				var d = math.lengthsq(math.lerp(e1, e2, t));
+				closestDistanceSq = math.select(closestDistanceSq, math.min(closestDistanceSq, d), lengthsq > math.FLT_MIN_NORMAL);
+			}
+			Assert.IsTrue(!float.IsNaN(closestDistanceSq));
+			return math.sqrt(closestDistanceSq);
+		}
+
+		public static float2 Linecast (float2 a, float2 b, NativeArray<float2> edges) {
+			var k = 1f;
+			for (int i = 0; i < edges.Length; i += 2) {
+				var e1 = edges[i];
+				var e2 = edges[i+1];
+				VectorMath.LineLineIntersectionFactors(a, b - a, e1, e2 - e1, out var t1, out var t2);
+				if (t2 >= 0 && t2 <= 1 && t1 > 0) {
+					k = math.min(k, t1);
+				}
+			}
+			return a + (b - a) * k;
+		}
+
+		public struct ControlParams {
+			public Vector3 p;
+			public float speed;
+			public float rotation;
+			public float maxDesiredWallDistance;
+			public float3 endOfPath;
+			public float3 facingDirectionAtEndOfPath;
+			public NativeArray<float2> edges;
+			public float3 nextCorner;
+			public float agentRadius;
+			public float remainingDistance;
+			public float3 closestOnNavmesh;
+			public DebugFlags debugFlags;
+			public NativeMovementPlane movementPlane;
+		}
+
+		/// <summary>
+		/// Finds the bounding box in which this controller is interested in navmesh edges.
+		///
+		/// The edges should be assigned to <see cref="ControlParams.edges"/>.
+		/// The bounding box is relative to the given movement plane.
+		/// </summary>
+		public static Bounds InterestingEdgeBounds (ref PIDMovement settings, float3 position, float3 nextCorner, float height, NativeMovementPlane plane) {
+			// Convert the position and next corner to local space, relative to the movement plane
+			var localPos = math.mul(math.conjugate(plane.rotation), position);
+			var localNextCorner = math.mul(math.conjugate(plane.rotation), nextCorner);
+			// Default bounds which extend from 1/2*height below the agent to the agent's head (assuming its pivot is at the agent's feet).
+			var localBounds = new Bounds(localPos + new float3(0, height * 0.25f, 0), new Vector3(0, 1.5f*height, 0));
+			// Don't allow the next corner to push the bounding box up or down too much, since that can let us include obstacle edges
+			// that are e.g. on a floor below or a floor above the agent. Especially if the agent is currently moving on a sloped surface.
+			localNextCorner.y = localPos.y;
+
+			localBounds.Encapsulate(localNextCorner);
+			// If an agent needs to make a full 180 degree turn, then we need a diameter instead of a radius.
+			// However since the agent gets a lower rotation speed the closer it gets to the desired rotation,
+			// this factor of two gets sort of compensated for already.
+			if (settings.rotationSpeed > 0) {
+				var approximateTurningDiameter = settings.speed / math.radians(settings.rotationSpeed); //2.0f * AnglePIDController.ApproximateTurningRadius(settings.followingStrength);
+				//
+				localBounds.Expand(new Vector3(1, 0, 1) * math.max(approximateTurningDiameter, settings.desiredWallDistance * OPTIMIZATION_ITERATIONS * STEP_MULTIPLIER));
+			}
+			return localBounds;
+		}
+
+		static float2 OffsetCornerForApproach (float2 position2D, float2 endOfPath2D, float2 facingDir2D, ref PIDMovement settings, float2 nextCorner2D, ref float gammaAngle, ref float gammaAngleWeight, DebugFlags debugFlags, ref CommandBuilder draw, NativeArray<float2> edges) {
+			var d1 = endOfPath2D - position2D;
+
+			// Cosine of the approach angle that is considered too steep to try to turn in an arc
+			const float STEEP_ANGLE_THRESHOLD_COS = -0.2f;
+
+			if (math.dot(math.normalizesafe(d1), facingDir2D) < STEEP_ANGLE_THRESHOLD_COS) {
+				// Too steep
+				return nextCorner2D;
+			}
+
+			// Line orthogonal to d1
+			var n1 = new float2(-d1.y, d1.x);
+
+			// Line orthogonal to facingDir2D
+			var n2 = new float2(-facingDir2D.y, facingDir2D.x);
+			var mid = (position2D + endOfPath2D) * 0.5f;
+
+			// Find the center of the circle which touches both the points endOfPath2D and position2D, and has a tangent parallel to facingDir2D at endOfPath2D.
+			var circleCenter = (float2)VectorMath.LineIntersectionPoint(mid, mid + n1, endOfPath2D, endOfPath2D + n2, out bool intersects);
+
+			if (!intersects) return nextCorner2D;
+
+			// Do not try to approach the destination with a large arc if there might be an obstacle in the way
+			// Check within a wedge and offset it sliightly backwards to take care of the case when the end of the path
+			// is right at the end of the navmesh. This is a common case when for example ordering an agent to interact
+			// with some prop.
+			//
+			//      Agent
+			//   |    |
+			// <-x----/
+			//   |
+			//
+			var distToObstacle = SmallestDistanceWithinWedge(endOfPath2D - 0.01f * facingDir2D, n2 - 0.1f * facingDir2D, -n2 - 0.1f * facingDir2D, 0.001f, edges);
+			var maxRadius = settings.leadInRadiusWhenApproachingDestination;
+			maxRadius = math.min(maxRadius, distToObstacle * 0.9f);
+			var circleRadius = math.length(circleCenter - endOfPath2D);
+
+			// Calculate the intersection point of the two tangents of the circle, one at endOfPath2D and one at position2D.
+			// Offset is the distance from endOfPath2D to the intersection point
+			var dot = math.abs(math.dot(math.normalizesafe(d1), n2));
+			var offset = 1.0f / math.sqrt(1 - dot*dot) * math.length(d1) * 0.5f;
+
+			// Tweak the offset slightly to account for the maximum radius.
+			// Limit the radius using a smooth thresholding function.
+			offset /= math.min(maxRadius, circleRadius);
+			offset = math.tanh(offset);
+			offset *= math.min(maxRadius, circleRadius);
+
+			// Offset the next corner backwards along the facing direction,
+			// so that the agent will approach the destination along a curve.
+			var newNextCorner2D = nextCorner2D - facingDir2D * offset;
+
+			if ((debugFlags & DebugFlags.ApproachWithOrientation) != 0) {
+				draw.xz.Circle(circleCenter, circleRadius, Color.blue);
+				draw.xz.Arrow(position2D, newNextCorner2D, Palette.Orange);
+			}
+
+			// If the new corner is not visible from the agent's current position,
+			// then return the original corner, as we do not want to try to walk into a wall.
+			if (math.lengthsq(Linecast(position2D, newNextCorner2D, edges) - newNextCorner2D) > 0.01f) {
+				return nextCorner2D;
+			} else {
+				return newNextCorner2D;
+			}
+		}
+
+		public static AnglePIDControlOutput2D Control (ref PIDMovement settings, float dt, ref ControlParams controlParams, ref CommandBuilder draw, out float maxDesiredWallDistance) {
+			if (dt <= 0) {
+				// If the game is paused, then do not move or rotate.
+				maxDesiredWallDistance = controlParams.maxDesiredWallDistance;
+				return new AnglePIDControlOutput2D {
+						   rotationDelta = 0,
+						   positionDelta = float2.zero,
+				};
+			}
+			var movementPlane = controlParams.movementPlane;
+			var position2D = movementPlane.ToPlane(controlParams.p, out float positionElevation);
+
+			// If we are drawing any debug information, push a matrix so that we can draw in local space.
+			// If not, skip pushing the matrix to improve performance.
+			if (controlParams.debugFlags != 0) draw.PushMatrix(math.mul(new float4x4(movementPlane.rotation, float3.zero), float4x4.Translate(new float3(0, positionElevation, 0))));
+
+			if ((controlParams.debugFlags & DebugFlags.Position) != 0) {
+				draw.xz.Cross(controlParams.closestOnNavmesh, 0.05f, Color.red);
+			}
+
+			var edges = controlParams.edges;
+			if ((controlParams.debugFlags & DebugFlags.Obstacles) != 0) {
+				draw.PushLineWidth(2);
+				draw.PushColor(Color.red);
+				for (int i = 0; i < edges.Length; i += 2) {
+					draw.xz.Line(edges[i], edges[i+1]);
+				}
+				draw.PopColor();
+				draw.PopLineWidth();
+			}
+
+			var nextCorner2D = movementPlane.ToPlane(controlParams.nextCorner);
+			float gamma = 0;
+			float gammaAngle = 0;
+			float gammaAngleWeight = 0;
+			// +Y is our forward direction, so add 90 degrees so that rotation2D = curveAngle means we are following the curve.
+			// Mathematically it makes much more sense if rotations are relative to the +X axis. So we use this convention internally.
+			var rotation2D = controlParams.rotation + Mathf.PI / 2;
+			var facingDir2D = math.normalizesafe(movementPlane.ToPlane(controlParams.facingDirectionAtEndOfPath));
+			bool isVeryCloseToEndOfPath = controlParams.remainingDistance < controlParams.agentRadius*0.1f;
+
+			if (!isVeryCloseToEndOfPath && settings.leadInRadiusWhenApproachingDestination > 0 && math.any(facingDir2D != 0)) {
+				var endOfPath2D = movementPlane.ToPlane(controlParams.endOfPath);
+				bool isAtLastCorner = math.lengthsq(endOfPath2D - nextCorner2D) <= 0.1f;
+				if (isAtLastCorner) {
+					var c1 = OffsetCornerForApproach(
+						position2D,
+						endOfPath2D,
+						facingDir2D,
+						ref settings,
+						nextCorner2D,
+						ref gammaAngle,
+						ref gammaAngleWeight,
+						controlParams.debugFlags,
+						ref draw,
+						edges
+						);
+					nextCorner2D = c1;
+
+					var simDx = settings.speed * 0.1f;
+					if (simDx > 0.001f) {
+						math.sincos(rotation2D, out var sin, out var cos);
+						var forward = new float2(cos, sin);
+						var c2 = OffsetCornerForApproach(
+							position2D + forward * simDx,
+							endOfPath2D,
+							facingDir2D,
+							ref settings,
+							nextCorner2D,
+							ref gammaAngle,
+							ref gammaAngleWeight,
+							DebugFlags.Nothing,
+							ref draw,
+							edges
+							);
+
+						// Calculate the number of radians between c1 and c2 from the agent's perspective.
+						// This is the amount that the agent must rotate to stay on the desired curve.
+						var s = VectorMath.Determinant(math.normalizesafe(c1 - position2D), math.normalizesafe(c2 - position2D));
+						gamma = math.asin(s)/simDx;
+					}
+				}
+			}
+
+			var desiredForwardClearanceRadius = settings.desiredWallDistance;
+			desiredForwardClearanceRadius = math.max(0, math.min(desiredForwardClearanceRadius, (controlParams.remainingDistance - desiredForwardClearanceRadius) / DESTINATION_CLEARANCE_FACTOR));
+			MarkerOptimizeDirection.Begin();
+
+			// In case the next corner is not visible from the agent's current position, then instead move towards the first intersection with an obstacle.
+			// This is important in some cases even when one would think that the next corner should be visible.
+			// This is because when unwrapping and flattening the funnel, the next corner may end up being move slightly due to various projections.
+			// This may cause it to end up inside a wall. If we didn't use a linecast here, the OptimizeDirection function
+			// would likely just give up and the agent would not stay away from the wall as it should.
+			nextCorner2D = Linecast(position2D, nextCorner2D, edges);
+
+			const float Pointiness = 2f;
+			var estimatedForward = OptimizeDirection(position2D, nextCorner2D, desiredForwardClearanceRadius, controlParams.remainingDistance, Pointiness, edges, draw, controlParams.debugFlags);
+			MarkerOptimizeDirection.End();
+
+			// Increase the maxDesiredWallDistance over time, to slowly push the agent away from walls.
+			maxDesiredWallDistance = controlParams.maxDesiredWallDistance + settings.speed * 0.1f * dt;
+			var desiredPositionClearance = maxDesiredWallDistance;
+			var signedDist = 0f;
+			var signedDistToClearArea = 0f;
+			maxDesiredWallDistance = math.min(maxDesiredWallDistance, desiredPositionClearance);
+
+			if ((controlParams.debugFlags & DebugFlags.Tangent) != 0) {
+				draw.Arrow(controlParams.p, controlParams.p + new Vector3(estimatedForward.x, 0, estimatedForward.y), Palette.Orange);
+			}
+
+			AnglePIDControlOutput2D output;
+			if (isVeryCloseToEndOfPath) {
+				// When we are really close to the endpoint, move directly towards the end and do not rotate (unless a facing direction has been set).
+
+				// Accelerate, but only up to the very low speed we use when we are very close to the endpoint.
+				// We must be able to accelerate here, as otherwise we may never reach the endpoint if we started
+				// very close to the endpoint with zero speed.
+				var speed = math.min(settings.Speed(controlParams.remainingDistance), settings.Accelerate(controlParams.speed, settings.slowdownTime, dt));
+
+				// TODO: Maybe add a settling mechanic. Once we are really close, lock the destination and do not change it until it gets a certain minimum distance away from the agent
+				// This would avoid the agent drifting without rotating to follow a destination that moves slowly.
+				var dirToEnd = nextCorner2D - position2D;
+				var distToEnd = math.length(dirToEnd);
+				if (math.any(facingDir2D != 0)) {
+					var desiredAngle = math.atan2(facingDir2D.y, facingDir2D.x);
+					var maxRotationDelta = dt * math.radians(settings.maxRotationSpeed);
+					output = new AnglePIDControlOutput2D {
+						rotationDelta = math.clamp(AstarMath.DeltaAngle(rotation2D, desiredAngle), -maxRotationDelta, maxRotationDelta),
+						// Convert back to a rotation convention where +Y is forward
+						targetRotation = desiredAngle - Mathf.PI / 2,
+						positionDelta = distToEnd > math.FLT_MIN_NORMAL ? dirToEnd * (dt * speed / distToEnd) : dirToEnd,
+					};
+				} else {
+					output = new AnglePIDControlOutput2D {
+						rotationDelta = 0,
+						// Convert back to a rotation convention where +Y is forward
+						targetRotation = rotation2D - Mathf.PI / 2,
+						positionDelta = distToEnd > math.FLT_MIN_NORMAL ? dirToEnd * (dt * speed / distToEnd) : dirToEnd,
+					};
+				}
+			} else {
+				var modifiedFollowingStrength = settings.CurveFollowingStrength(signedDistToClearArea, desiredPositionClearance, controlParams.remainingDistance);
+				var curveAngle = math.atan2(estimatedForward.y, estimatedForward.x);
+
+				var minimumRotationSpeed = 0f;
+				// If we are not perfectly facing our desired direction, we need to rotate to face it.
+				// We try to ensure we will not hit any obstacles by checking for nearby obstacles
+				// in the direction we are moving. If there are any obstacles, we can calculate
+				// the approximate rotation speed we need to have to avoid them.
+				//
+				// If we are very close to our desired facing direction, we skip this check
+				// to improve performance.
+				if (math.abs(AstarMath.DeltaAngle(curveAngle, rotation2D)) > math.PI*0.001f) {
+					math.sincos(rotation2D, out var sin, out var cos);
+					var forward = new float2(cos, sin);
+					var closestWithinWedge = SmallestDistanceWithinWedge(position2D, estimatedForward, forward, controlParams.agentRadius*0.1f, edges);
+
+					if ((controlParams.debugFlags & DebugFlags.ForwardClearance) != 0 && float.IsFinite(closestWithinWedge)) {
+						draw.xz.Arc(position2D, position2D + forward * closestWithinWedge, position2D + estimatedForward, Palette.Purple);
+					}
+
+					if (closestWithinWedge > 0.001f && closestWithinWedge*1.01f < controlParams.remainingDistance) {
+						const float SAFETY_FACTOR = 2.0f;
+						minimumRotationSpeed = math.rcp(closestWithinWedge) * SAFETY_FACTOR;
+					}
+				}
+
+				MarkerPID.Begin();
+				output = AnglePIDController.Control(
+					ref settings,
+					modifiedFollowingStrength,
+					rotation2D,
+					curveAngle + AstarMath.DeltaAngle(curveAngle, gammaAngle) * gammaAngleWeight,
+					gamma,
+					signedDist,
+					controlParams.speed,
+					controlParams.remainingDistance,
+					minimumRotationSpeed,
+					controlParams.speed < settings.speed*0.1f,
+					dt
+					);
+				// Convert back to a rotation convention where +Y is forward
+				output.targetRotation -= Mathf.PI / 2;
+				MarkerPID.End();
+			}
+			if (controlParams.debugFlags != 0) draw.PopMatrix();
+			return output;
+		}
+	}
+
+	/// <summary>
+	/// Implements a PID controller for the angular velocity of an agent following a curve.
+	///
+	/// The PID controller is formulated for small angles (see https://en.wikipedia.org/wiki/Small-angle_approximation), but extends well to large angles.
+	/// For small angles, if y(t) is the curve/agent position, then y'(t) is the angle and y''(t) is the angular velocity.
+	/// This controller outputs an angular velocity, meaning it controls y''(t).
+	///
+	/// See https://en.wikipedia.org/wiki/PID_controller
+	/// </summary>
+	public static class AnglePIDController {
+		const float DampingRatio = 1.0f;
+
+		/// <summary>
+		/// An approximate turning radius the agent will have in an open space.
+		///
+		/// This is based on the PID controller in the <see cref="Control"/> method.
+		/// </summary>
+		public static float ApproximateTurningRadius (float followingStrength) {
+			// With dampingRatio = 1, this will result in critical damping
+			var alpha = followingStrength;
+			var beta = 2 * math.sqrt(math.abs(alpha)) * DampingRatio;
+
+			// Some sort of mean value
+			// If a character turns around, the angleToCurveError will go from math.PI to 0.
+			const float angleToCurveError = math.PI * 0.5f;
+
+			return 1.0f/(beta * angleToCurveError);
+		}
+
+		/// <summary>
+		/// Given a speed and a rotation speed, what is the approximate corresponding following strength.
+		///
+		/// This is based on the PID controller in the <see cref="Control"/> method.
+		/// </summary>
+		public static float RotationSpeedToFollowingStrength (float speed, float maxRotationSpeed) {
+			// Using the following identity:
+			// turningRadius = speed/rotationSpeed
+			// and using the implementation for ApproximateTurningRadius, we can solve for the rotation speed
+			// and we get the expression below.
+
+			// Note that we use a different angleToCurveError here compared to in ApproximateTurningRadius.
+			// This is because here we use the maximum angleToCurveError that could happen, while in ApproximateTurningRadius
+			// we use an average value. This is reasonable because the input to this method is
+			// the maximum rotation speed, not the average rotation speed.
+			const float angleToCurveError = math.PI;
+			var k = maxRotationSpeed / (2.0f * angleToCurveError * speed * DampingRatio);
+			var alpha = k * k;
+			return alpha;
+		}
+
+		public static float FollowingStrengthToRotationSpeed (float followingStrength) {
+			return 1.0f / (ApproximateTurningRadius(followingStrength) * 0.5f);
+		}
+
+		/// <summary>
+		/// How much to rotate and move in order to smoothly follow a given curve.
+		///
+		/// If the maximum rotation speed (settings.maxRotationSpeed) would be exceeded, the agent will slow down to avoid exceeding it (up to a point).
+		///
+		/// Returns: A control value that can be used to move the agent.
+		/// </summary>
+		/// <param name="settings">Various movement settings</param>
+		/// <param name="followingStrength">The integral term of the PID controller. The higher this value is, the quicker the agent will try to align with the curve.</param>
+		/// <param name="angle">The current direction of the agent, in radians.</param>
+		/// <param name="curveAngle">The angle of the curve tangent at the nearest point, in radians.</param>
+		/// <param name="curveCurvature">The curvature of the curve at the nearest point. Positive values means the curve is turning to the left, negative values means the curve is turning to the right.</param>
+		/// <param name="curveDistanceSigned">The signed distance from the agent to the curve. Positive values means the agent is to the right of the curve, negative values means the agent is to the left of the curve.</param>
+		/// <param name="speed">How quickly the agent should move. In meters/second.</param>
+		/// <param name="remainingDistance">The remaining distance to where the agent should stop. In meters.</param>
+		/// <param name="minRotationSpeed">The minimum rotation speed of the agent. In radians/second. Unless the agent does not desire to rotate at all, it will rotate at least this fast.</param>
+		/// <param name="isStationary">Should be true if the agent is currently standing still (or close to it). This allows it to rotate in place.</param>
+		/// <param name="dt">How long the current time-step is. In seconds.</param>
+		public static AnglePIDControlOutput2D Control (ref PIDMovement settings, float followingStrength, float angle, float curveAngle, float curveCurvature, float curveDistanceSigned, float speed, float remainingDistance, float minRotationSpeed, bool isStationary, float dt) {
+			Assert.IsTrue(math.isfinite(angle));
+			Assert.IsTrue(math.isfinite(curveAngle));
+			Assert.IsTrue(math.isfinite(curveDistanceSigned));
+			Assert.IsTrue(math.isfinite(curveCurvature));
+			Assert.IsTrue(minRotationSpeed >= 0);
+
+			// With dampingRatio = 1, this will result in critical damping
+			var alpha = followingStrength;
+			var beta = 2 * math.sqrt(math.abs(alpha)) * DampingRatio;
+			var gamma = 1.0f;
+			var angleToCurveError = AstarMath.DeltaAngle(angle, curveAngle);
+			var angleTowardsCurve = curveAngle + math.sign(curveDistanceSigned) * math.PI * 0.5f;
+			var deltaAngleTowardsCurve = AstarMath.DeltaAngle(angle, angleTowardsCurve); // TODO: Divide by PI/2?
+
+			// Desired primary rotation in radians per meter
+			var alphaAngle = alpha * math.abs(curveDistanceSigned) * deltaAngleTowardsCurve;
+			// Desired primary rotation during this timestep
+			var alphaAngleDelta = alphaAngle * speed * dt;
+
+			// Desired secondary rotation in radians per meter
+			var betaAngle = beta * angleToCurveError;
+
+			// Assuming that an agent is stationary, the rotation of the agent will reach a steady state after a short while (alphaAngle + betaAngle = 0).
+			// This is the remaining angle we have left until we reach that steady state.
+			var denominator = beta  + alpha * math.abs(curveDistanceSigned);
+			var remainingAngle = denominator > math.FLT_MIN_NORMAL ? (betaAngle + alphaAngle)/denominator : 0;
+			Assert.IsTrue(math.isfinite(remainingAngle));
+			float.IsFinite(remainingAngle);
+
+			// If the agent has to rotate *a lot* then stop moving and rotate in-place.
+			// Once we are rotating in place, we should continue doing that until we are almost facing the desired direction.
+			isStationary = settings.allowRotatingOnSpot && (math.abs(remainingAngle) > math.PI*0.6666f || (isStationary && math.abs(remainingAngle) > 0.1f));
+			if (isStationary) {
+				var newSpeed = settings.Accelerate(speed, settings.slowdownTimeWhenTurningOnSpot, -dt);
+				var maxOnSpotRotationSpeed = math.radians(settings.maxOnSpotRotationSpeed);
+				var canRotateInOneStep = maxOnSpotRotationSpeed*dt > math.abs(remainingAngle);
+				if (newSpeed > 0 && !canRotateInOneStep) {
+					// Slow down as quickly as possible
+					return AnglePIDControlOutput2D.WithMovementAtEnd(
+						currentRotation: angle,
+						targetRotation: angle,
+						rotationDelta: 0,
+						moveDistance: newSpeed * dt
+						);
+				} else {
+					// If we are rotating in place, rotate with the maximum rotation speed
+					return AnglePIDControlOutput2D.WithMovementAtEnd(
+						currentRotation: angle,
+						targetRotation: angle + remainingAngle,
+						rotationDelta: math.clamp(remainingAngle, -maxOnSpotRotationSpeed*dt, maxOnSpotRotationSpeed*dt),
+						// Check if we can rotate in place in one time-step. If so, skip standing still for this time-step.
+						moveDistance: canRotateInOneStep ? speed * dt : 0.0f
+						);
+				}
+			}
+
+			speed = math.min(settings.Speed(remainingDistance), settings.Accelerate(speed, settings.slowdownTime, dt));
+
+			if (math.abs(angleToCurveError) > math.PI*0.5f) {
+				// Ensures that if the agent is moving in the completely wrong direction, it will not continue doing that
+				// because the alpha term tells it to move left, and the beta term tells it to move right, cancelling each other out.
+				alphaAngleDelta = 0;
+			}
+
+			if (math.abs(betaAngle) > 0.0001f) {
+				betaAngle = math.max(math.abs(betaAngle), minRotationSpeed) * math.sign(betaAngle);
+			}
+
+			var betaAngleDelta = betaAngle * speed * dt;
+			// The weights are "how much we want to rotate this timestep, divided by the maximum amount of rotation that is allowed"
+			// This is used to avoid overshooting when following strengths are very high or the fps is low.
+			var alphaWeight = math.abs(alphaAngleDelta / deltaAngleTowardsCurve);
+			var betaWeight = math.abs(betaAngleDelta / angleToCurveError);
+			var gammaWeight = 1.0f;
+			var directionComponentInCurveDirection = math.max(0, math.cos(angleToCurveError));
+			var speedMultiplier = 1.0f;
+			var moveDistance = speed * speedMultiplier * dt;
+			var curvatureIntegral = curveCurvature * moveDistance;
+			var gammaAngleDelta = gamma * curvatureIntegral * directionComponentInCurveDirection;
+			// Don't allow individual contributions to contribute more than their limit (e.g. overshooting their rotation target).
+			// But still keep the relative contribution proportions the same.
+			var overflowWeight = math.max(1f, math.max(alphaWeight, math.max(betaWeight, gammaWeight)));
+			var angleDelta = (gammaAngleDelta + betaAngleDelta + alphaAngleDelta) / overflowWeight;
+
+			// If we would have rotated too quickly, slow down the agent
+			var maxRotationSpeed = math.radians(settings.maxRotationSpeed);
+			var rotationMultiplier = math.max(0.1f, math.min(1.0f, maxRotationSpeed*dt / math.abs(angleDelta)));
+
+			Assert.IsTrue(math.isfinite(angle));
+			Assert.IsTrue(math.isfinite(rotationMultiplier));
+			Assert.IsTrue(math.isfinite(angleDelta));
+			Assert.IsTrue(math.isfinite(moveDistance));
+
+			return new AnglePIDControlOutput2D(
+				currentRotation: angle,
+				targetRotation: angle + remainingAngle,
+				rotationDelta: angleDelta * rotationMultiplier,
+				moveDistance: moveDistance * rotationMultiplier
+				);
+		}
+	}
+}
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDMovement.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDMovement.cs.meta
new file mode 100644
index 0000000..9080220
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDMovement.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 2d19693db79b5eedab18d1362b58bca1
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs
new file mode 100644
index 0000000..3695b37
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs
@@ -0,0 +1,48 @@
+using UnityEngine;
+using Unity.Mathematics;
+using UnityEngine.Assertions;
+using Pathfinding.Util;
+
+namespace Pathfinding.PID {
+	public struct AnglePIDControlOutput2D {
+		/// <summary>How much to rotate in a single time-step. In radians.</summary>
+		public float rotationDelta;
+		public float targetRotation;
+		/// <summary>How much to move in a single time-step. In world units.</summary>
+		public float2 positionDelta;
+
+		public AnglePIDControlOutput2D(float currentRotation, float targetRotation, float rotationDelta, float moveDistance) {
+			var midpointRotation = currentRotation + rotationDelta * 0.5f;
+			math.sincos(midpointRotation, out float s, out float c);
+			this.rotationDelta = rotationDelta;
+			this.positionDelta = new float2(c, s) * moveDistance;
+			this.targetRotation = targetRotation;
+		}
+
+		public static AnglePIDControlOutput2D WithMovementAtEnd (float currentRotation, float targetRotation, float rotationDelta, float moveDistance) {
+			var finalRotation = currentRotation + rotationDelta;
+			math.sincos(finalRotation, out float s, out float c);
+			return new AnglePIDControlOutput2D {
+					   rotationDelta = rotationDelta,
+					   targetRotation = targetRotation,
+					   positionDelta = new float2(c, s) * moveDistance,
+			};
+		}
+	}
+
+	public struct AnglePIDControlOutput {
+		/// <summary>How much to rotate in a single time-step</summary>
+		public quaternion rotationDelta;
+		/// <summary>How much to move in a single time-step. In world units.</summary>
+		public float3 positionDelta;
+		public float maxDesiredWallDistance;
+
+		public AnglePIDControlOutput(NativeMovementPlane movementPlane, AnglePIDControlOutput2D control2D) {
+			this.rotationDelta = movementPlane.ToWorldRotationDelta(-control2D.rotationDelta);
+			this.positionDelta = movementPlane.ToWorld(control2D.positionDelta, 0);
+			this.maxDesiredWallDistance = 0;
+			Assert.IsTrue(math.all(math.isfinite(rotationDelta.value)));
+			Assert.IsTrue(math.all(math.isfinite(positionDelta)));
+		}
+	}
+}
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs.meta
new file mode 100644
index 0000000..5641b86
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/Control/PIDUtilities.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: e3e01131736f2ac179398e1f408f94eb
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: