+ astar project

16 files changed, 4356 insertions, 0 deletions

diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgent.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgent.cs
new file mode 100644
index 0000000..3b6ed49
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgent.cs
@@ -0,0 +1,4 @@
+
+// This file has been removed from the project. Since UnityPackages cannot
+// delete files, only replace them, this message is left here to prevent old
+// files from causing compiler errors
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgent.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgent.cs.meta
new file mode 100644
index 0000000..521b101
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgent.cs.meta
@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 5a85e178962ba475ca424001ea4c13ca
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgentBurst.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgentBurst.cs
new file mode 100644
index 0000000..5af0518
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgentBurst.cs
@@ -0,0 +1,1998 @@
+using UnityEngine;
+using System.Collections.Generic;
+
+namespace Pathfinding.RVO {
+	using Pathfinding;
+	using Pathfinding.Util;
+	using Unity.Burst;
+	using Unity.Jobs;
+	using Unity.Mathematics;
+	using Unity.Collections;
+	using Unity.IL2CPP.CompilerServices;
+	using Pathfinding.Drawing;
+	using Pathfinding.ECS.RVO;
+	using static Unity.Burst.CompilerServices.Aliasing;
+	using Unity.Profiling;
+	using System.Diagnostics;
+
+	[BurstCompile(CompileSynchronously = false, FloatMode = FloatMode.Fast)]
+	public struct JobRVOPreprocess : IJob {
+		[ReadOnly]
+		public SimulatorBurst.AgentData agentData;
+
+		[ReadOnly]
+		public SimulatorBurst.AgentOutputData previousOutput;
+
+		[WriteOnly]
+		public SimulatorBurst.TemporaryAgentData temporaryAgentData;
+
+		public int startIndex;
+		public int endIndex;
+
+		public void Execute () {
+			for (int i = startIndex; i < endIndex; i++) {
+				if (!agentData.version[i].Valid) continue;
+
+				// Manually controlled overrides the agent being locked.
+				// If one for some reason uses them at the same time.
+				var locked = agentData.locked[i] & !agentData.manuallyControlled[i];
+
+				if (locked) {
+					temporaryAgentData.desiredTargetPointInVelocitySpace[i] = float2.zero;
+					temporaryAgentData.desiredVelocity[i] = float3.zero;
+					temporaryAgentData.currentVelocity[i] = float3.zero;
+				} else {
+					var desiredTargetPointInVelocitySpace = agentData.movementPlane[i].ToPlane(agentData.targetPoint[i] - agentData.position[i]);
+					temporaryAgentData.desiredTargetPointInVelocitySpace[i] = desiredTargetPointInVelocitySpace;
+
+					// Estimate our current velocity
+					// This is necessary because other agents need to know
+					// how this agent is moving to be able to avoid it
+					var currentVelocity = math.normalizesafe(previousOutput.targetPoint[i] - agentData.position[i]) * previousOutput.speed[i];
+
+					// Calculate the desired velocity from the point we want to reach
+					temporaryAgentData.desiredVelocity[i] = agentData.movementPlane[i].ToWorld(math.normalizesafe(desiredTargetPointInVelocitySpace) * agentData.desiredSpeed[i], 0);
+
+					var collisionNormal = math.normalizesafe(agentData.collisionNormal[i]);
+					// Check if the velocity is going into the wall
+					// If so: remove that component from the velocity
+					// Note: if the collisionNormal is zero then the dot prodct will produce a zero as well and nothing will happen.
+					float dot = math.dot(currentVelocity, collisionNormal);
+					currentVelocity -= math.min(0, dot) * collisionNormal;
+					temporaryAgentData.currentVelocity[i] = currentVelocity;
+				}
+			}
+		}
+	}
+
+	/// <summary>
+	/// Inspired by StarCraft 2's avoidance of locked units.
+	/// See: http://www.gdcvault.com/play/1014514/AI-Navigation-It-s-Not
+	/// </summary>
+	[BurstCompile(FloatMode = FloatMode.Fast)]
+	public struct JobHorizonAvoidancePhase1 : Pathfinding.Jobs.IJobParallelForBatched {
+		[ReadOnly]
+		public SimulatorBurst.AgentData agentData;
+
+		[ReadOnly]
+		public NativeArray<float2> desiredTargetPointInVelocitySpace;
+
+		[ReadOnly]
+		public NativeArray<int> neighbours;
+
+		public SimulatorBurst.HorizonAgentData horizonAgentData;
+
+		public CommandBuilder draw;
+
+		public bool allowBoundsChecks { get { return true; } }
+
+		/// <summary>
+		/// Super simple bubble sort.
+		/// TODO: This will be replaced by a better implementation from the Unity.Collections library when that is stable.
+		/// </summary>
+		static void Sort<T>(NativeSlice<T> arr, NativeSlice<float> keys) where T : struct {
+			bool changed = true;
+
+			while (changed) {
+				changed = false;
+				for (int i = 0; i < arr.Length - 1; i++) {
+					if (keys[i] > keys[i+1]) {
+						var tmp = keys[i];
+						var tmp2 = arr[i];
+						keys[i] = keys[i+1];
+						keys[i+1] = tmp;
+						arr[i] = arr[i+1];
+						arr[i+1] = tmp2;
+						changed = true;
+					}
+				}
+			}
+		}
+
+
+		/// <summary>Calculates the shortest difference between two given angles given in radians.</summary>
+		public static float DeltaAngle (float current, float target) {
+			float num = Mathf.Repeat(target - current, math.PI*2);
+
+			if (num > math.PI) {
+				num -= math.PI*2;
+			}
+			return num;
+		}
+
+		public void Execute (int startIndex, int count) {
+			NativeArray<float> angles = new NativeArray<float>(SimulatorBurst.MaxNeighbourCount*2, Allocator.Temp);
+			NativeArray<int> deltas = new NativeArray<int>(SimulatorBurst.MaxNeighbourCount*2, Allocator.Temp);
+
+			for (int i = startIndex; i < startIndex + count; i++) {
+				if (!agentData.version[i].Valid) continue;
+
+				if (agentData.locked[i] || agentData.manuallyControlled[i]) {
+					horizonAgentData.horizonSide[i] = 0;
+					horizonAgentData.horizonMinAngle[i] = 0;
+					horizonAgentData.horizonMaxAngle[i] = 0;
+					continue;
+				}
+
+				float minAngle = 0;
+				float maxAngle = 0;
+
+				float desiredAngle = math.atan2(desiredTargetPointInVelocitySpace[i].y, desiredTargetPointInVelocitySpace[i].x);
+
+				int eventCount = 0;
+
+				int inside = 0;
+
+				float radius = agentData.radius[i];
+
+				var position = agentData.position[i];
+				var movementPlane = agentData.movementPlane[i];
+
+				var agentNeighbours = neighbours.Slice(i*SimulatorBurst.MaxNeighbourCount, SimulatorBurst.MaxNeighbourCount);
+				for (int j = 0; j < agentNeighbours.Length && agentNeighbours[j] != -1; j++) {
+					var other = agentNeighbours[j];
+					if (!agentData.locked[other] && !agentData.manuallyControlled[other]) continue;
+
+					var relativePosition = movementPlane.ToPlane(agentData.position[other] - position);
+					float dist = math.length(relativePosition);
+
+					float angle = math.atan2(relativePosition.y, relativePosition.x) - desiredAngle;
+					float deltaAngle;
+
+					var otherRadius = agentData.radius[other];
+					if (dist < radius + otherRadius) {
+						// Collision
+						deltaAngle = math.PI * 0.49f;
+					} else {
+						// One degree
+						const float AngleMargin = math.PI / 180f;
+						deltaAngle = math.asin((radius + otherRadius)/dist) + AngleMargin;
+					}
+
+					float aMin = DeltaAngle(0, angle - deltaAngle);
+					float aMax = aMin + DeltaAngle(aMin, angle + deltaAngle);
+
+					if (aMin < 0 && aMax > 0) inside++;
+
+					angles[eventCount] = aMin;
+					deltas[eventCount] = 1;
+					eventCount++;
+					angles[eventCount] = aMax;
+					deltas[eventCount] = -1;
+					eventCount++;
+				}
+
+				// If no angle range includes angle 0 then we are already done
+				if (inside == 0) {
+					horizonAgentData.horizonSide[i] = 0;
+					horizonAgentData.horizonMinAngle[i] = 0;
+					horizonAgentData.horizonMaxAngle[i] = 0;
+					continue;
+				}
+
+				// Sort the events by their angle in ascending order
+				Sort(deltas.Slice(0, eventCount), angles.Slice(0, eventCount));
+
+				// Find the first index for which the angle is positive
+				int firstPositiveIndex = 0;
+				for (; firstPositiveIndex < eventCount; firstPositiveIndex++) if (angles[firstPositiveIndex] > 0) break;
+
+				// Walk in the positive direction from angle 0 until the end of the group of angle ranges that include angle 0
+				int tmpInside = inside;
+				int tmpIndex = firstPositiveIndex;
+				for (; tmpIndex < eventCount; tmpIndex++) {
+					tmpInside += deltas[tmpIndex];
+					if (tmpInside == 0) break;
+				}
+				maxAngle = tmpIndex == eventCount ? math.PI : angles[tmpIndex];
+
+				// Walk in the negative direction from angle 0 until the end of the group of angle ranges that include angle 0
+				tmpInside = inside;
+				tmpIndex = firstPositiveIndex - 1;
+				for (; tmpIndex >= 0; tmpIndex--) {
+					tmpInside -= deltas[tmpIndex];
+					if (tmpInside == 0) break;
+				}
+				minAngle = tmpIndex == -1 ? -math.PI : angles[tmpIndex];
+
+				//horizonBias = -(minAngle + maxAngle);
+
+				// Indicates that a new side should be chosen. The "best" one will be chosen later.
+				if (horizonAgentData.horizonSide[i] == 0) horizonAgentData.horizonSide[i] = 2;
+				//else horizonBias = math.PI * horizonSide;
+
+				horizonAgentData.horizonMinAngle[i] = minAngle + desiredAngle;
+				horizonAgentData.horizonMaxAngle[i] = maxAngle + desiredAngle;
+			}
+		}
+	}
+
+	/// <summary>
+	/// Inspired by StarCraft 2's avoidance of locked units.
+	/// See: http://www.gdcvault.com/play/1014514/AI-Navigation-It-s-Not
+	/// </summary>
+	[BurstCompile(FloatMode = FloatMode.Fast)]
+	public struct JobHorizonAvoidancePhase2 : Pathfinding.Jobs.IJobParallelForBatched {
+		[ReadOnly]
+		public NativeArray<int> neighbours;
+		[ReadOnly]
+		public NativeArray<AgentIndex> versions;
+		public NativeArray<float3> desiredVelocity;
+		public NativeArray<float2> desiredTargetPointInVelocitySpace;
+
+		[ReadOnly]
+		public NativeArray<NativeMovementPlane> movementPlane;
+
+		public SimulatorBurst.HorizonAgentData horizonAgentData;
+
+		public bool allowBoundsChecks => false;
+
+		public void Execute (int startIndex, int count) {
+			for (int i = startIndex; i < startIndex + count; i++) {
+				if (!versions[i].Valid) continue;
+
+				// Note: Assumes this code is run synchronous (i.e not included in the double buffering part)
+				//offsetVelocity = (position - Position) / simulator.DeltaTime;
+
+				if (horizonAgentData.horizonSide[i] == 0) {
+					continue;
+				}
+
+				if (horizonAgentData.horizonSide[i] == 2) {
+					float sum = 0;
+					var agentNeighbours = neighbours.Slice(i*SimulatorBurst.MaxNeighbourCount, SimulatorBurst.MaxNeighbourCount);
+					for (int j = 0; j < agentNeighbours.Length && agentNeighbours[j] != -1; j++) {
+						var other = agentNeighbours[j];
+						var otherHorizonBias = -(horizonAgentData.horizonMinAngle[other] + horizonAgentData.horizonMaxAngle[other]);
+						sum += otherHorizonBias;
+					}
+					var horizonBias = -(horizonAgentData.horizonMinAngle[i] + horizonAgentData.horizonMaxAngle[i]);
+					sum += horizonBias;
+
+					horizonAgentData.horizonSide[i] = sum < 0 ? -1 : 1;
+				}
+
+				float bestAngle = horizonAgentData.horizonSide[i] < 0 ? horizonAgentData.horizonMinAngle[i] : horizonAgentData.horizonMaxAngle[i];
+				float2 desiredDirection;
+				math.sincos(bestAngle, out desiredDirection.y, out desiredDirection.x);
+				desiredVelocity[i] = movementPlane[i].ToWorld(math.length(desiredVelocity[i]) * desiredDirection, 0);
+				desiredTargetPointInVelocitySpace[i] = math.length(desiredTargetPointInVelocitySpace[i]) * desiredDirection;
+			}
+		}
+	}
+
+	[BurstCompile(FloatMode = FloatMode.Fast)]
+	public struct JobHardCollisions<MovementPlaneWrapper> : Pathfinding.Jobs.IJobParallelForBatched where MovementPlaneWrapper : struct, IMovementPlaneWrapper {
+		[ReadOnly]
+		public SimulatorBurst.AgentData agentData;
+		[ReadOnly]
+		public NativeArray<int> neighbours;
+		[WriteOnly]
+		public NativeArray<float2> collisionVelocityOffsets;
+
+		public float deltaTime;
+		public bool enabled;
+
+		/// <summary>
+		/// How aggressively hard collisions are resolved.
+		/// Should be a value between 0 and 1.
+		/// </summary>
+		const float CollisionStrength = 0.8f;
+
+		public bool allowBoundsChecks => false;
+
+		public void Execute (int startIndex, int count) {
+			if (!enabled) {
+				for (int i = startIndex; i < startIndex + count; i++) {
+					collisionVelocityOffsets[i] = float2.zero;
+				}
+				return;
+			}
+
+			for (int i = startIndex; i < startIndex + count; i++) {
+				if (!agentData.version[i].Valid || agentData.locked[i]) {
+					collisionVelocityOffsets[i] = float2.zero;
+					continue;
+				}
+
+				var agentNeighbours = neighbours.Slice(i*SimulatorBurst.MaxNeighbourCount, SimulatorBurst.MaxNeighbourCount);
+				var radius = agentData.radius[i];
+				var totalOffset = float2.zero;
+				float totalWeight = 0;
+
+				var position = agentData.position[i];
+				var movementPlane = new MovementPlaneWrapper();
+				movementPlane.Set(agentData.movementPlane[i]);
+
+				for (int j = 0; j < agentNeighbours.Length && agentNeighbours[j] != -1; j++) {
+					var other = agentNeighbours[j];
+					var relativePosition = movementPlane.ToPlane(position - agentData.position[other]);
+
+					var dirSqrLength = math.lengthsq(relativePosition);
+					var combinedRadius = agentData.radius[other] + radius;
+					if (dirSqrLength < combinedRadius*combinedRadius && dirSqrLength > 0.00000001f) {
+						// Collision
+						var dirLength = math.sqrt(dirSqrLength);
+						var normalizedDir = relativePosition * (1.0f / dirLength);
+
+						// Overlap amount
+						var weight = combinedRadius - dirLength;
+
+						// Position offset required to make the agents not collide anymore
+						var offset = normalizedDir * weight;
+						// In a later step a weighted average will be taken so that the average offset is extracted
+						var weightedOffset = offset * weight;
+
+						totalOffset += weightedOffset;
+						totalWeight += weight;
+					}
+				}
+
+				var offsetVelocity = totalOffset * (1.0f / (0.0001f + totalWeight));
+				offsetVelocity *= (CollisionStrength * 0.5f) / deltaTime;
+
+				collisionVelocityOffsets[i] = offsetVelocity;
+			}
+		}
+	}
+
+	[BurstCompile(CompileSynchronously = false, FloatMode = FloatMode.Fast)]
+	public struct JobRVOCalculateNeighbours<MovementPlaneWrapper> : Pathfinding.Jobs.IJobParallelForBatched where MovementPlaneWrapper : struct, IMovementPlaneWrapper {
+		[ReadOnly]
+		public SimulatorBurst.AgentData agentData;
+
+		[ReadOnly]
+		public RVOQuadtreeBurst quadtree;
+
+		public NativeArray<int> outNeighbours;
+
+		[WriteOnly]
+		public SimulatorBurst.AgentOutputData output;
+
+		public bool allowBoundsChecks { get { return false; } }
+
+		public void Execute (int startIndex, int count) {
+			NativeArray<float> neighbourDistances = new NativeArray<float>(SimulatorBurst.MaxNeighbourCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+
+			for (int i = startIndex; i < startIndex + count; i++) {
+				if (!agentData.version[i].Valid) continue;
+				CalculateNeighbours(i, outNeighbours, neighbourDistances);
+			}
+		}
+
+		void CalculateNeighbours (int agentIndex, NativeArray<int> neighbours, NativeArray<float> neighbourDistances) {
+			int maxNeighbourCount = math.min(SimulatorBurst.MaxNeighbourCount, agentData.maxNeighbours[agentIndex]);
+			// Write the output starting at this index in the neighbours array
+			var outputIndex = agentIndex * SimulatorBurst.MaxNeighbourCount;
+
+			quadtree.QueryKNearest(new RVOQuadtreeBurst.QuadtreeQuery {
+				position = agentData.position[agentIndex],
+				speed = agentData.maxSpeed[agentIndex],
+				agentRadius = agentData.radius[agentIndex],
+				timeHorizon = agentData.agentTimeHorizon[agentIndex],
+				outputStartIndex = outputIndex,
+				maxCount = maxNeighbourCount,
+				result = neighbours,
+				resultDistances = neighbourDistances,
+			});
+
+			int numNeighbours = 0;
+			while (numNeighbours < maxNeighbourCount && math.isfinite(neighbourDistances[numNeighbours])) numNeighbours++;
+			output.numNeighbours[agentIndex] = numNeighbours;
+
+			MovementPlaneWrapper movementPlane = default;
+			movementPlane.Set(agentData.movementPlane[agentIndex]);
+			movementPlane.ToPlane(agentData.position[agentIndex], out float localElevation);
+
+			// Filter out invalid neighbours
+			for (int i = 0; i < numNeighbours; i++) {
+				int otherIndex = neighbours[outputIndex + i];
+				// Interval along the y axis in which the agents overlap
+				movementPlane.ToPlane(agentData.position[otherIndex], out float otherElevation);
+				float maxY = math.min(localElevation + agentData.height[agentIndex], otherElevation + agentData.height[otherIndex]);
+				float minY = math.max(localElevation, otherElevation);
+
+				// The agents cannot collide if they are on different y-levels.
+				// Also do not avoid the agent itself.
+				// Apply the layer masks for agents.
+				// Use binary OR to reduce branching.
+				if ((maxY < minY) | (otherIndex == agentIndex) | (((int)agentData.collidesWith[agentIndex] & (int)agentData.layer[otherIndex]) == 0)) {
+					numNeighbours--;
+					neighbours[outputIndex + i] = neighbours[outputIndex + numNeighbours];
+					i--;
+				}
+			}
+
+			// Add a token indicating the size of the neighbours list
+			if (numNeighbours < SimulatorBurst.MaxNeighbourCount) neighbours[outputIndex + numNeighbours] = -1;
+		}
+	}
+
+	/// <summary>
+	/// Calculates if the agent has reached the end of its path and if its blocked from further progress towards it.
+	///
+	/// If many agents have the same destination they can often end up crowded around a single point.
+	/// It is often desirable to detect this and mark all agents around that destination as having at least
+	/// partially reached the end of their paths.
+	///
+	/// This job uses the following heuristics to determine this:
+	///
+	/// 1. If an agent wants to move in a particular direction, but there's another agent in the way that makes it have to reduce its velocity,
+	///     the other agent is considered to be "blocking" the current agent.
+	/// 2. If the agent is within a small distance of the destination
+	///        THEN it is considered to have reached the end of its path.
+	/// 3. If the agent is blocked by another agent,
+	///        AND the other agent is blocked by this agent in turn,
+	///        AND if the destination is between the two agents,
+	///        THEN the the agent is considered to have reached the end of its path.
+	/// 4. If the agent is blocked by another agent which has reached the end of its path,
+	///        AND this agent is is moving slowly
+	///        AND this agent cannot move furter forward than 50% of its radius.
+	///        THEN the agent is considered to have reached the end of its path.
+	///
+	/// Heuristics 2 and 3 are calculated initially, and then using heuristic 4 the set of agents which have reached their destinations expands outwards.
+	///
+	/// These heuristics are robust enough that they can be used even if for example the agents are stuck in a winding maze
+	/// and only one agent is actually able to reach the destination.
+	///
+	/// This job doesn't affect the movement of the agents by itself.
+	/// However, it is built with the intention that the FlowFollowingStrength parameter will be set
+	/// elsewhere to 1 for agents which have reached the end of their paths. This will make the agents stop gracefully
+	/// when the end of their paths is crowded instead of continuing to try to desperately reach the destination.
+	/// </summary>
+	[BurstCompile(CompileSynchronously = false, FloatMode = FloatMode.Fast)]
+	public struct JobDestinationReached<MovementPlaneWrapper>: IJob where MovementPlaneWrapper : struct, IMovementPlaneWrapper {
+		[ReadOnly]
+		public SimulatorBurst.AgentData agentData;
+
+		[ReadOnly]
+		public SimulatorBurst.TemporaryAgentData temporaryAgentData;
+
+		[ReadOnly]
+		public SimulatorBurst.ObstacleData obstacleData;
+
+		public SimulatorBurst.AgentOutputData output;
+		public int numAgents;
+		public CommandBuilder draw;
+
+		private static readonly ProfilerMarker MarkerInvert = new ProfilerMarker("InvertArrows");
+		private static readonly ProfilerMarker MarkerAlloc = new ProfilerMarker("Alloc");
+		private static readonly ProfilerMarker MarkerFirstPass = new ProfilerMarker("FirstPass");
+
+		struct TempAgentData {
+			public bool blockedAndSlow;
+			public float distToEndSq;
+		}
+
+		public void Execute () {
+			MarkerAlloc.Begin();
+			for (int agentIndex = 0; agentIndex < numAgents; agentIndex++) {
+				output.effectivelyReachedDestination[agentIndex] = ReachedEndOfPath.NotReached;
+			}
+
+			// For each agent, store which agents it blocks
+			var inArrows = new NativeArray<int>(agentData.position.Length*SimulatorBurst.MaxBlockingAgentCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+			// Number of agents that each agent blocks
+			var inArrowCounts = new NativeArray<int>(agentData.position.Length, Allocator.Temp, NativeArrayOptions.ClearMemory);
+			var que = new NativeCircularBuffer<int>(16, Allocator.Temp);
+			// True for an agent if it is in the queue, or if it should never be queued again
+			var queued = new NativeArray<bool>(numAgents, Allocator.Temp, NativeArrayOptions.ClearMemory);
+			var tempData = new NativeArray<TempAgentData>(numAgents, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+			MarkerAlloc.End();
+			MarkerInvert.Begin();
+
+			for (int agentIndex = 0; agentIndex < numAgents; agentIndex++) {
+				if (!agentData.version[agentIndex].Valid) continue;
+				for (int i = 0; i < SimulatorBurst.MaxBlockingAgentCount; i++) {
+					var blockingAgentIndex = output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount + i];
+					if (blockingAgentIndex == -1) break;
+					var count = inArrowCounts[blockingAgentIndex];
+					if (count >= SimulatorBurst.MaxBlockingAgentCount) continue;
+					inArrows[blockingAgentIndex*SimulatorBurst.MaxBlockingAgentCount + count] = agentIndex;
+					inArrowCounts[blockingAgentIndex] = count+1;
+				}
+			}
+			MarkerInvert.End();
+
+			MarkerFirstPass.Begin();
+			for (int agentIndex = 0; agentIndex < numAgents; agentIndex++) {
+				if (!agentData.version[agentIndex].Valid) continue;
+
+				var position = agentData.position[agentIndex];
+				var movementPlane = agentData.movementPlane[agentIndex];
+				var ourSpeed = output.speed[agentIndex];
+				var ourEndOfPath = agentData.endOfPath[agentIndex];
+
+				// Ignore if destination is not set
+				if (!math.isfinite(ourEndOfPath.x)) continue;
+
+				var distToEndSq = math.lengthsq(movementPlane.ToPlane(ourEndOfPath - position, out float endOfPathElevationDifference));
+				var ourHeight = agentData.height[agentIndex];
+				var reachedEndOfPath = false;
+				var flowFollowing = false;
+				var ourRadius = agentData.radius[agentIndex];
+				var forwardClearance = output.forwardClearance[agentIndex];
+
+				// Heuristic 2
+				if (distToEndSq < ourRadius*ourRadius*(0.5f*0.5f) && endOfPathElevationDifference < ourHeight && endOfPathElevationDifference > -ourHeight*0.5f) {
+					reachedEndOfPath = true;
+				}
+
+				var closeToBlocked = forwardClearance < ourRadius*0.5f;
+				var slowish = ourSpeed*ourSpeed < math.max(0.01f*0.01f, math.lengthsq(temporaryAgentData.desiredVelocity[agentIndex])*0.25f);
+				var blockedAndSlow = closeToBlocked && slowish;
+				tempData[agentIndex] = new TempAgentData {
+					blockedAndSlow = blockedAndSlow,
+					distToEndSq = distToEndSq
+				};
+
+				// Heuristic 3
+				for (int i = 0; i < SimulatorBurst.MaxBlockingAgentCount; i++) {
+					var blockingAgentIndex = output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount + i];
+					if (blockingAgentIndex == -1) break;
+
+					var otherPosition = agentData.position[blockingAgentIndex];
+					var distBetweenAgentsSq = math.lengthsq(movementPlane.ToPlane(position - otherPosition));
+					var circleRadius = (math.sqrt(distBetweenAgentsSq) + ourRadius + agentData.radius[blockingAgentIndex])*0.5f;
+					var endWithinCircle = math.lengthsq(movementPlane.ToPlane(ourEndOfPath - 0.5f*(position + otherPosition))) < circleRadius*circleRadius;
+					if (endWithinCircle) {
+						// Check if the other agent has an arrow pointing to this agent (i.e. it is blocked by this agent)
+						var loop = false;
+						for (int j = 0; j < SimulatorBurst.MaxBlockingAgentCount; j++) {
+							var arrowFromAgent = inArrows[agentIndex*SimulatorBurst.MaxBlockingAgentCount + j];
+							if (arrowFromAgent == -1) break;
+							if (arrowFromAgent == blockingAgentIndex) {
+								loop = true;
+								break;
+							}
+						}
+
+						if (loop) {
+							flowFollowing = true;
+
+							if (blockedAndSlow) {
+								reachedEndOfPath = true;
+							}
+						}
+					}
+				}
+
+				var effectivelyReached = reachedEndOfPath ? ReachedEndOfPath.Reached : (flowFollowing ? ReachedEndOfPath.ReachedSoon : ReachedEndOfPath.NotReached);
+				if (effectivelyReached != output.effectivelyReachedDestination[agentIndex]) {
+					output.effectivelyReachedDestination[agentIndex] = effectivelyReached;
+
+					if (effectivelyReached == ReachedEndOfPath.Reached) {
+						// Mark this agent as queued to prevent it from being added to the queue again.
+						queued[agentIndex] = true;
+
+						// Changing to the Reached flag may affect the calculations for other agents.
+						// So we iterate over all agents that may be affected and enqueue them again.
+						var count = inArrowCounts[agentIndex];
+						for (int i = 0; i < count; i++) {
+							var inArrow = inArrows[agentIndex*SimulatorBurst.MaxBlockingAgentCount + i];
+							if (!queued[inArrow]) que.PushEnd(inArrow);
+						}
+					}
+				}
+			}
+			MarkerFirstPass.End();
+
+
+			int iteration = 0;
+			while (que.Length > 0) {
+				var agentIndex = que.PopStart();
+				iteration++;
+				// If we are already at the reached stage, the result can never change.
+				if (output.effectivelyReachedDestination[agentIndex] == ReachedEndOfPath.Reached) continue;
+				queued[agentIndex] = false;
+
+				var ourSpeed = output.speed[agentIndex];
+				var ourEndOfPath = agentData.endOfPath[agentIndex];
+				// Ignore if destination is not set
+				if (!math.isfinite(ourEndOfPath.x)) continue;
+
+				var ourPosition = agentData.position[agentIndex];
+				var blockedAndSlow = tempData[agentIndex].blockedAndSlow;
+				var distToEndSq = tempData[agentIndex].distToEndSq;
+				var ourRadius = agentData.radius[agentIndex];
+				var reachedEndOfPath = false;
+				var flowFollowing = false;
+
+				// Heuristic 4
+				for (int i = 0; i < SimulatorBurst.MaxBlockingAgentCount; i++) {
+					var blockingAgentIndex = output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount + i];
+					if (blockingAgentIndex == -1) break;
+
+					var otherEndOfPath = agentData.endOfPath[blockingAgentIndex];
+					var otherRadius = agentData.radius[blockingAgentIndex];
+
+					// Check if the other agent has a destination in roughly the same position as this agent.
+					// If we are further from the destination we tolarate larger deviations.
+					var endOfPathsOverlapping = math.lengthsq(otherEndOfPath - ourEndOfPath) <= distToEndSq*(0.5f*0.5f);
+					var otherReached = output.effectivelyReachedDestination[blockingAgentIndex] == ReachedEndOfPath.Reached;
+
+					if (otherReached && (endOfPathsOverlapping || math.lengthsq(ourEndOfPath - agentData.position[blockingAgentIndex]) < math.lengthsq(ourRadius+otherRadius))) {
+						var otherSpeed = output.speed[blockingAgentIndex];
+						flowFollowing |= math.min(ourSpeed, otherSpeed) < 0.01f;
+						reachedEndOfPath |= blockedAndSlow;
+					}
+				}
+
+				var effectivelyReached = reachedEndOfPath ? ReachedEndOfPath.Reached : (flowFollowing ? ReachedEndOfPath.ReachedSoon : ReachedEndOfPath.NotReached);
+				// We do not check for all things that are checked in the first pass. So incorporate the previous information by taking the max.
+				effectivelyReached = (ReachedEndOfPath)math.max((int)effectivelyReached, (int)output.effectivelyReachedDestination[agentIndex]);
+
+				if (effectivelyReached != output.effectivelyReachedDestination[agentIndex]) {
+					output.effectivelyReachedDestination[agentIndex] = effectivelyReached;
+
+					if (effectivelyReached == ReachedEndOfPath.Reached) {
+						// Mark this agent as queued to prevent it from being added to the queue again.
+						queued[agentIndex] = true;
+
+						// Changes to the Reached flag may affect the calculations for other agents.
+						// So we iterate over all agents that may be affected and enqueue them again.
+						var count = inArrowCounts[agentIndex];
+						for (int i = 0; i < count; i++) {
+							var inArrow = inArrows[agentIndex*SimulatorBurst.MaxBlockingAgentCount + i];
+							if (!queued[inArrow]) que.PushEnd(inArrow);
+						}
+					}
+				}
+			}
+		}
+	}
+
+	// Note: FloatMode should not be set to Fast because that causes inaccuracies which can lead to
+	// agents failing to avoid walls sometimes.
+	[BurstCompile(CompileSynchronously = true, FloatMode = FloatMode.Default)]
+	public struct JobRVO<MovementPlaneWrapper> : Pathfinding.Jobs.IJobParallelForBatched where MovementPlaneWrapper : struct, IMovementPlaneWrapper {
+		[ReadOnly]
+		public SimulatorBurst.AgentData agentData;
+
+		[ReadOnly]
+		public SimulatorBurst.TemporaryAgentData temporaryAgentData;
+
+		[ReadOnly]
+		public NavmeshEdges.NavmeshBorderData navmeshEdgeData;
+
+		[WriteOnly]
+		public SimulatorBurst.AgentOutputData output;
+
+		public float deltaTime;
+		public float symmetryBreakingBias;
+		public float priorityMultiplier;
+		public bool useNavmeshAsObstacle;
+
+		public bool allowBoundsChecks { get { return true; } }
+
+		const int MaxObstacleCount = 50;
+
+		public CommandBuilder draw;
+
+		public void Execute (int startIndex, int batchSize) {
+			ExecuteORCA(startIndex, batchSize);
+		}
+
+		struct SortByKey : IComparer<int> {
+			public UnsafeSpan<float> keys;
+
+			public int Compare (int x, int y) {
+				return keys[x].CompareTo(keys[y]);
+			}
+		}
+
+		/// <summary>
+		/// Sorts the array in place using insertion sort.
+		/// This is a stable sort.
+		/// See: http://en.wikipedia.org/wiki/Insertion_sort
+		///
+		/// Used only because Unity.Collections.NativeSortExtension.Sort seems to have some kind of code generation bug when using Burst 1.8.2, causing it to throw exceptions.
+		/// </summary>
+		static void InsertionSort<T, U>(UnsafeSpan<T> data, U comparer) where T : unmanaged where U : IComparer<T> {
+			for (int i = 1; i < data.Length; i++) {
+				var value = data[i];
+				int j = i - 1;
+				while (j >= 0 && comparer.Compare(data[j], value) > 0) {
+					data[j + 1] = data[j];
+					j--;
+				}
+				data[j + 1] = value;
+			}
+		}
+
+		private static readonly ProfilerMarker MarkerConvertObstacles1 = new ProfilerMarker("RVOConvertObstacles1");
+		private static readonly ProfilerMarker MarkerConvertObstacles2 = new ProfilerMarker("RVOConvertObstacles2");
+
+		/// <summary>
+		/// Generates ORCA half-planes for all obstacles near the agent.
+		/// For more details refer to the ORCA (Optimal Reciprocal Collision Avoidance) paper.
+		///
+		/// This function takes in several arrays which are just used for temporary data. This is to avoid the overhead of allocating the arrays once for every agent.
+		/// </summary>
+		void GenerateObstacleVOs (int agentIndex, NativeList<int> adjacentObstacleIdsScratch, NativeArray<int2> adjacentObstacleVerticesScratch, NativeArray<float> segmentDistancesScratch, NativeArray<int> sortedVerticesScratch, NativeArray<ORCALine> orcaLines, NativeArray<int> orcaLineToAgent, [NoAlias] ref int numLines, [NoAlias] in MovementPlaneWrapper movementPlane, float2 optimalVelocity) {
+			if (!useNavmeshAsObstacle) return;
+
+			var localPosition = movementPlane.ToPlane(agentData.position[agentIndex], out var agentElevation);
+			var agentHeight = agentData.height[agentIndex];
+			var agentRadius = agentData.radius[agentIndex];
+			var obstacleRadius = agentRadius * 0.01f;
+			var inverseObstacleTimeHorizon = math.rcp(agentData.obstacleTimeHorizon[agentIndex]);
+
+			ExpectNotAliased(in agentData.collisionNormal, in agentData.position);
+
+			var hierarchicalNodeIndex = agentData.hierarchicalNodeIndex[agentIndex];
+			if (hierarchicalNodeIndex == -1) return;
+
+			var size = (obstacleRadius + agentRadius + agentData.obstacleTimeHorizon[agentIndex] * agentData.maxSpeed[agentIndex]) * new float3(2, 0, 2);
+			size.y = agentData.height[agentIndex] * 2f;
+			var bounds = new Bounds(new Vector3(localPosition.x, agentElevation, localPosition.y), size);
+			var boundingRadiusSq = math.lengthsq(bounds.extents);
+			adjacentObstacleIdsScratch.Clear();
+
+			var worldBounds = movementPlane.ToWorld(bounds);
+			navmeshEdgeData.GetObstaclesInRange(hierarchicalNodeIndex, worldBounds, adjacentObstacleIdsScratch);
+
+#if UNITY_EDITOR
+			if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.Obstacles)) {
+				draw.PushMatrix(movementPlane.matrix);
+				draw.PushMatrix(new float4x4(
+					new float4(1, 0, 0, 0),
+					new float4(0, 0, -1, 0),
+					new float4(0, 1, 0, 0),
+					new float4(0, 0, 0, 1)
+					));
+				draw.WireBox(bounds, Color.blue);
+				draw.PopMatrix();
+				draw.PopMatrix();
+			}
+#endif
+
+			// TODO: For correctness all obstacles should be added in nearest-to-farthest order.
+			// This loop should be split up.
+			for (int oi = 0; oi < adjacentObstacleIdsScratch.Length; oi++) {
+				MarkerConvertObstacles1.Begin();
+				var obstacleId = adjacentObstacleIdsScratch[oi];
+
+				var obstacleAllocations = navmeshEdgeData.obstacleData.obstacles[obstacleId];
+				var vertices = navmeshEdgeData.obstacleData.obstacleVertices.GetSpan(obstacleAllocations.verticesAllocation);
+				var groups = navmeshEdgeData.obstacleData.obstacleVertexGroups.GetSpan(obstacleAllocations.groupsAllocation);
+				int vertexOffset = 0;
+				int candidateVertexCount = 0;
+				for (int i = 0; i < groups.Length; i++) {
+					var group = groups[i];
+					// Check if the group does not overlap with our bounds at all
+					if (!math.all((group.boundsMx >= worldBounds.min) & (group.boundsMn <= worldBounds.max))) {
+						vertexOffset += group.vertexCount;
+						continue;
+					}
+
+
+					var startVertex = vertexOffset;
+					var endVertex = vertexOffset + group.vertexCount - 1;
+					if (endVertex >= adjacentObstacleVerticesScratch.Length) {
+						// Too many vertices. Skip remaining vertices.
+						break;
+					}
+
+					for (int vi = startVertex; vi < startVertex + group.vertexCount; vi++) {
+						// X coordinate is the index of the previous vertex, the y coordinate is the next vertex
+						adjacentObstacleVerticesScratch[vi] = new int2(vi - 1, vi + 1);
+					}
+					// UnityEngine.Assertions.Assert.AreEqual(vertexCount, endVertex + 1);
+
+					// Patch the start and end vertices to be correct.
+					// In a chain the last vertex doesn't start a new segment so we just make it loop back on itself.
+					// In a loop the last vertex connects to the first vertex.
+					adjacentObstacleVerticesScratch[startVertex] = new int2(group.type == ObstacleType.Loop ? endVertex : startVertex, adjacentObstacleVerticesScratch[startVertex].y);
+					adjacentObstacleVerticesScratch[endVertex] = new int2(adjacentObstacleVerticesScratch[endVertex].x, group.type == ObstacleType.Loop ? startVertex : endVertex);
+
+					for (int vi = 0; vi < group.vertexCount; vi++) {
+						var vertex = vertices[vi + vertexOffset];
+						int next = adjacentObstacleVerticesScratch[vi + startVertex].y;
+						var pos = movementPlane.ToPlane(vertex) - localPosition;
+						var nextPos = movementPlane.ToPlane(vertices[next]) - localPosition;
+						var dir = nextPos - pos;
+						var closestT = ClosestPointOnSegment(pos, dir / math.lengthsq(dir), float2.zero, 0, 1);
+						var dist = math.lengthsq(pos + dir*closestT);
+						segmentDistancesScratch[vi + startVertex] = dist;
+
+						if (dist <= boundingRadiusSq && candidateVertexCount < sortedVerticesScratch.Length) {
+							sortedVerticesScratch[candidateVertexCount] = vi + startVertex;
+							candidateVertexCount++;
+						}
+					}
+
+					vertexOffset += group.vertexCount;
+				}
+
+				MarkerConvertObstacles1.End();
+
+				MarkerConvertObstacles2.Begin();
+				// Sort obstacle segments by distance from the agent
+				InsertionSort(sortedVerticesScratch.AsUnsafeSpan().Slice(0, candidateVertexCount), new SortByKey {
+					keys = segmentDistancesScratch.AsUnsafeSpan().Slice(0, vertexOffset)
+				});
+
+				for (int i = 0; i < candidateVertexCount; i++) {
+					// In the unlikely event that we exceed the maximum number of obstacles, we just skip the remaining ones.
+					if (numLines >= MaxObstacleCount) break;
+
+					// Processing the obstacle defined by v1 and v2
+					//
+					// v0                v3
+					//   \               /
+					//    \             /
+					//    v1 ========= v2
+					//
+					var v1Index = sortedVerticesScratch[i];
+
+					// If the obstacle is too far away, we can skip it.
+					// Since the obstacles are sorted by distance we can break here.
+					if (segmentDistancesScratch[v1Index] > 0.25f*size.x*size.x) break;
+
+					var v0Index = adjacentObstacleVerticesScratch[v1Index].x;
+					var v2Index = adjacentObstacleVerticesScratch[v1Index].y;
+					if (v2Index == v1Index) continue;
+					var v3Index = adjacentObstacleVerticesScratch[v2Index].y;
+					UnityEngine.Assertions.Assert.AreNotEqual(v1Index, v3Index);
+					UnityEngine.Assertions.Assert.AreNotEqual(v0Index, v2Index);
+
+					var v0 = vertices[v0Index];
+					var v1 = vertices[v1Index];
+					var v2 = vertices[v2Index];
+					var v3 = vertices[v3Index];
+
+					var v0Position = movementPlane.ToPlane(v0) - localPosition;
+					var v1Position = movementPlane.ToPlane(v1, out var e1) - localPosition;
+					var v2Position = movementPlane.ToPlane(v2, out var e2) - localPosition;
+					var v3Position = movementPlane.ToPlane(v3) - localPosition;
+
+					// Assume the obstacle has the same height as the agent, then check if they overlap along the elevation axis.
+					if (math.max(e1, e2) + agentHeight < agentElevation || math.min(e1, e2) > agentElevation + agentHeight) {
+						// The obstacle is not in the agent's elevation range. Ignore it.
+						continue;
+					}
+
+					var length = math.length(v2Position - v1Position);
+					if (length < 0.0001f) continue;
+					var segmentDir = (v2Position - v1Position) * math.rcp(length);
+
+					if (det(segmentDir, -v1Position) > obstacleRadius) {
+						// Agent is significantly on the wrong side of the segment (on the "inside"). Ignore it.
+						continue;
+					}
+
+					// Check if this velocity obstacle completely behind previously added ORCA lines.
+					// If so, this obstacle is redundant and we can ignore it.
+					// This is not just a performance optimization. Using the ORCA lines for closer
+					// obstacles is better since obstacles further away can add ORCA lines that
+					// restrict the velocity space unnecessarily. The ORCA line is more conservative than the VO.
+					bool alreadyCovered = false;
+
+					const float EPSILON = 0.0001f;
+					for (var j = 0; j < numLines; j++) {
+						var line = orcaLines[j];
+						if (
+							// Check if this velocity-obstacle is completely inside the previous ORCA line's infeasible half-plane region.
+							det(inverseObstacleTimeHorizon * v1Position - line.point, line.direction) - inverseObstacleTimeHorizon * obstacleRadius >= -EPSILON &&
+							det(inverseObstacleTimeHorizon * v2Position - line.point, line.direction) - inverseObstacleTimeHorizon * obstacleRadius >= -EPSILON
+							) {
+							alreadyCovered = true;
+							break;
+						}
+					}
+					if (alreadyCovered) {
+						continue;
+					}
+
+					var obstacleOptimizationVelocity = float2.zero;
+					var distanceAlongSegment = math.dot(obstacleOptimizationVelocity - v1Position, segmentDir);
+					var closestPointOnSegment = v1Position + distanceAlongSegment * segmentDir;
+					var distanceToLineSq = math.lengthsq(closestPointOnSegment - obstacleOptimizationVelocity);
+					var distanceToSegmentSq = math.lengthsq((v1Position + math.clamp(distanceAlongSegment, 0, length) * segmentDir));
+
+					var v1Convex = leftOrColinear(v1Position - v0Position, segmentDir);
+					var v2Convex = leftOrColinear(segmentDir, v3Position - v2Position);
+
+					if (distanceToSegmentSq < obstacleRadius*obstacleRadius) {
+						if (distanceAlongSegment < 0.0f) {
+							// Collision with left vertex, ignore if the vertex is not convex
+							if (v1Convex) {
+								orcaLineToAgent[numLines] = -1;
+								orcaLines[numLines++] = new ORCALine {
+									point = -v1Position * 0.1f,
+									direction = math.normalizesafe(rot90(v1Position)),
+								};
+							}
+						} else if (distanceAlongSegment > length) {
+							// Collision with right vertex
+							// Ignore if the vertex is not convex, or if it will be taken care of
+							// by the neighbour obstacle segment.
+							if (v2Convex && leftOrColinear(v2Position, v3Position - v2Position)) {
+								orcaLineToAgent[numLines] = -1;
+								orcaLines[numLines++] = new ORCALine {
+									point = -v2Position * 0.1f,
+									direction = math.normalizesafe(rot90(v2Position)),
+								};
+							}
+						} else {
+							// Collision with segment
+							orcaLineToAgent[numLines] = -1;
+							orcaLines[numLines++] = new ORCALine {
+								point = -closestPointOnSegment * 0.1f,
+								direction = -segmentDir,
+							};
+						}
+						continue;
+					}
+
+					// Represents rays starting points on the VO circles, going in a tangent direction away from the agent.
+					float2 leftLegDirection, rightLegDirection;
+
+					if ((distanceAlongSegment < 0 || distanceAlongSegment > 1) && distanceToLineSq <= obstacleRadius*obstacleRadius) {
+						// Obliquely viewed so that the circle around one of the vertices is all that is visible from p. p = obstacleOptimizationVelocity
+						//     _____________________________  _ _ _ _ _ _ _ _ _ _ _ _
+						//   _/     \_               _/     \_
+						//  /         \             /         \
+						//  |    v1   |             |    v2   |
+						//  \_       _/             \_       _/          p
+						//    \_____/_________________\_____/ _ _ _ _ _ _ _ _ _ _ _ _
+
+						// Collapse segment to a single point, making sure that v0 and v3 are still the neighbouring vertices.
+						if (distanceAlongSegment < 0) {
+							// Collapse to v1
+							// Ignore if not convex
+							if (!v1Convex) continue;
+							v3Position = v2Position;
+							v2Position = v1Position;
+							v2Convex = v1Convex;
+						} else {
+							// Collapse to v2
+							if (!v2Convex) continue;
+							v0Position = v1Position;
+							v1Position = v2Position;
+							v1Convex = v2Convex;
+						}
+						var vertexDistSq = math.lengthsq(v1Position);
+						// Distance from p to the points where the legs (tangents) touch the circle around the vertex.
+						float leg = math.sqrt(vertexDistSq - obstacleRadius*obstacleRadius);
+						var posNormal = new float2(-v1Position.y, v1Position.x);
+						// These become normalized
+						leftLegDirection = (v1Position*leg + posNormal*obstacleRadius) / vertexDistSq;
+						rightLegDirection = (v1Position*leg - posNormal*obstacleRadius) / vertexDistSq;
+					} else {
+						// This is the common case (several valid positions of p are shown). p = obstacleOptimizationVelocity
+						//
+						//        p
+						//     _____________________________
+						//   _/     \_               _/     \_
+						//  /         \             /         \
+						//  |    v1   |             |    v2   |
+						//  \_       _/             \_       _/
+						//    \_____/_________________\_____/
+						//
+						//                       p                   p
+
+						if (v1Convex) {
+							var vertexDistSq = math.lengthsq(v1Position);
+							float leg = math.sqrt(vertexDistSq - obstacleRadius*obstacleRadius);
+							var posNormal = new float2(-v1Position.y, v1Position.x);
+							// This becomes normalized
+							leftLegDirection = (v1Position*leg + posNormal*obstacleRadius) / vertexDistSq;
+						} else {
+							leftLegDirection = -segmentDir;
+						}
+
+						if (v2Convex) {
+							var vertexDistSq = math.lengthsq(v2Position);
+							float leg = math.sqrt(vertexDistSq - obstacleRadius*obstacleRadius);
+							var posNormal = new float2(-v2Position.y, v2Position.x);
+							rightLegDirection = (v2Position*leg - posNormal*obstacleRadius) / vertexDistSq;
+						} else {
+							rightLegDirection = segmentDir;
+						}
+					}
+
+					// Legs should never point into the obstacle for legs added by convex vertices.
+					// The neighbouring vertex will add a better obstacle for those cases.
+					//
+					// In that case we replace the legs with the neighbouring segments, and if the closest
+					// point is on those segments we know we can ignore them because the
+					// neighbour will handle it.
+					//
+					// It's important that we don't include the case when they are colinear,
+					// because if v1=v0 (or v2=v3), which can happen at the end of a chain, the
+					// determinant will always be zero and so they will seem colinear.
+					//
+					// Note: One might think that this should apply to all vertices, not just convex ones.
+					// Consider this case where you might think a non-convex vertices otherwise would
+					// cause 'ghost' obstacles:
+					//   ___
+					//  |  | A
+					//  |  |
+					//  |   \
+					//  |____\ B
+					//       <-X
+					//
+					// If X is an agent, moving to the left. It could get stuck against the segment A.
+					// This is because the vertex between A and B is concave, and it will generate a leg
+					// pointing downwards.
+					//
+					// However, this does not cause a problem in practice. Because if the horizontal segment at the bottom is added first (as it should be)
+					// then A and B will be discarded since they will be completely behind the ORCA line added by the horizontal segment.
+					bool isLeftLegForeign = false;
+					bool isRightLegForeign = false;
+					if (v1Convex && left(leftLegDirection, v0Position - v1Position)) {
+						// Left leg points into obstacle
+						leftLegDirection = v0Position - v1Position;
+						isLeftLegForeign = true;
+					}
+
+					if (v2Convex && right(rightLegDirection, v3Position - v2Position)) {
+						// Right leg points into obstacle
+						rightLegDirection = v3Position - v2Position;
+						isRightLegForeign = true;
+					}
+
+
+					// The velocity obstacle for this segment consists of a left leg, right leg,
+					// a cutoff line, and two circular arcs where the legs and the cutoff line join together.
+					// LeftLeg                                   RightLeg
+					//    \      _____________________________      /
+					//     \   _/     \_               _/     \_   /
+					//      \ /         \             /         \ /
+					//       \|    v1   |             |    v2   |/
+					//        \_       _/             \_       _/
+					//          \_____/_________________\_____/
+					//                     Cutoff Line
+					//
+					// In case only one vertex makes up the obstacle then we instead have just a left leg, right leg, and a single circular arc.
+					//
+					//  LeftLeg           RightLeg
+					//    \      _____      /
+					//     \   _/     \_   /
+					//      \ /         \ /
+					//       \|         |/
+					//        \_       _/
+					//          \_____/
+					//
+
+
+					// We first check if the velocity will be projected on those circular segments.
+					var leftCutoff = inverseObstacleTimeHorizon * v1Position;
+					var rightCutoff = inverseObstacleTimeHorizon * v2Position;
+					var cutoffDir = rightCutoff - leftCutoff;
+					var cutoffLength = math.lengthsq(cutoffDir);
+
+					// Projection on the cutoff line (between 0 and 1 if the projection is on the cutoff segment)
+					var t = cutoffLength <= 0.00001f ? 0.5f : math.dot(optimalVelocity - leftCutoff, cutoffDir)/cutoffLength;
+					// Negative if the closest point on the rays reprensenting the legs is before the ray starts
+					var tLeft = math.dot(optimalVelocity - leftCutoff, leftLegDirection);
+					var tRight = math.dot(optimalVelocity - rightCutoff, rightLegDirection);
+
+
+					// Check if the projected velocity is on the circular arcs
+					if ((t < 0.0f && tLeft < 0.0f) || (t > 1.0f && tRight < 0.0f) || (cutoffLength <= 0.00001f && tLeft < 0.0f && tRight < 0.0f)) {
+						var arcCenter = t <= 0.5f ? leftCutoff : rightCutoff;
+
+						var unitW = math.normalizesafe(optimalVelocity - arcCenter);
+						orcaLineToAgent[numLines] = -1;
+						orcaLines[numLines++] = new ORCALine {
+							point = arcCenter + obstacleRadius * inverseObstacleTimeHorizon * unitW,
+							direction = new float2(unitW.y, -unitW.x),
+						};
+						continue;
+					}
+
+					// If the closest point is not on the arcs, then we project it on the legs or the cutoff line and pick the closest one.
+					// Note that all these distances should be reduced by obstacleRadius, but we only compare the values, so this doesn't matter.
+					float distToCutoff = (t > 1.0f || t < 0.0f || cutoffLength < 0.0001f ? math.INFINITY : math.lengthsq(optimalVelocity - (leftCutoff + t * cutoffDir)));
+					float distToLeftLeg = (tLeft < 0.0f ? math.INFINITY : math.lengthsq(optimalVelocity - (leftCutoff + tLeft * leftLegDirection)));
+					float distToRightLeg = (tRight < 0.0f ? math.INFINITY : math.lengthsq(optimalVelocity - (rightCutoff + tRight * rightLegDirection)));
+					var selected = 0;
+					var mn = distToCutoff;
+					if (distToLeftLeg < mn) {
+						mn = distToLeftLeg;
+						selected = 1;
+					}
+					if (distToRightLeg < mn) {
+						mn = distToRightLeg;
+						selected = 2;
+					}
+
+					if (selected == 0) {
+						// Project on cutoff line
+						orcaLineToAgent[numLines] = -1;
+						orcaLines[numLines++] = new ORCALine {
+							point = leftCutoff + obstacleRadius * inverseObstacleTimeHorizon * new float2(segmentDir.y, -segmentDir.x),
+							direction = -segmentDir,
+						};
+					} else if (selected == 1) {
+						if (!isLeftLegForeign) {
+							orcaLineToAgent[numLines] = -1;
+							orcaLines[numLines++] = new ORCALine {
+								point = leftCutoff + obstacleRadius * inverseObstacleTimeHorizon * new float2(-leftLegDirection.y, leftLegDirection.x),
+								direction = leftLegDirection,
+							};
+						}
+					} else if (selected == 2) {
+						if (!isRightLegForeign) {
+							orcaLineToAgent[numLines] = -1;
+							orcaLines[numLines++] = new ORCALine {
+								point = rightCutoff + obstacleRadius * inverseObstacleTimeHorizon * new float2(rightLegDirection.y, -rightLegDirection.x),
+								direction = -rightLegDirection,
+							};
+						}
+					}
+				}
+				MarkerConvertObstacles2.End();
+			}
+		}
+
+		public void ExecuteORCA (int startIndex, int batchSize) {
+			int endIndex = startIndex + batchSize;
+
+			NativeArray<ORCALine> orcaLines = new NativeArray<ORCALine>(SimulatorBurst.MaxNeighbourCount + MaxObstacleCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+			NativeArray<ORCALine> scratchBuffer = new NativeArray<ORCALine>(SimulatorBurst.MaxNeighbourCount + MaxObstacleCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+			NativeArray<float> segmentDistancesScratch = new NativeArray<float>(SimulatorBurst.MaxObstacleVertices, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+			NativeArray<int> sortedVerticesScratch = new NativeArray<int>(SimulatorBurst.MaxObstacleVertices, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+			NativeArray<int2> adjacentObstacleVertices = new NativeArray<int2>(4 * SimulatorBurst.MaxObstacleVertices, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+			NativeArray<int> orcaLineToAgent = new NativeArray<int>(SimulatorBurst.MaxNeighbourCount + MaxObstacleCount, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
+			NativeList<int> adjacentObstacleIdsScratch = new NativeList<int>(16, Allocator.Temp);
+
+			for (int agentIndex = startIndex; agentIndex < endIndex; agentIndex++) {
+				if (!agentData.version[agentIndex].Valid) continue;
+
+				if (agentData.manuallyControlled[agentIndex]) {
+					output.speed[agentIndex] = agentData.desiredSpeed[agentIndex];
+					output.targetPoint[agentIndex] = agentData.targetPoint[agentIndex];
+					output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount] = -1;
+					continue;
+				}
+
+				var position = agentData.position[agentIndex];
+
+				if (agentData.locked[agentIndex]) {
+					output.speed[agentIndex] = 0;
+					output.targetPoint[agentIndex] = position;
+					output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount] = -1;
+					continue;
+				}
+
+				MovementPlaneWrapper movementPlane = default;
+				movementPlane.Set(agentData.movementPlane[agentIndex]);
+
+				// The RVO algorithm assumes we will continue to
+				// move in roughly the same direction
+				float2 optimalVelocity = movementPlane.ToPlane(temporaryAgentData.currentVelocity[agentIndex]);
+				int numLines = 0;
+				// TODO: Obstacles are typically behind agents, so it's better to add the agent orca lines first to improve culling.
+				// However, the 3D optimization program requires obstacle lines to be added first. Not to mention that the culling
+				// is not strictly accurate for fixed obstacle since they cannot be moved backwards by the 3D linear program.
+				GenerateObstacleVOs(agentIndex, adjacentObstacleIdsScratch, adjacentObstacleVertices, segmentDistancesScratch, sortedVerticesScratch, orcaLines, orcaLineToAgent, ref numLines, in movementPlane, optimalVelocity);
+				int numFixedLines = numLines;
+
+				var neighbours = temporaryAgentData.neighbours.Slice(agentIndex*SimulatorBurst.MaxNeighbourCount, SimulatorBurst.MaxNeighbourCount);
+
+				float agentTimeHorizon = agentData.agentTimeHorizon[agentIndex];
+				float inverseAgentTimeHorizon = math.rcp(agentTimeHorizon);
+				float priority = agentData.priority[agentIndex];
+
+				var localPosition = movementPlane.ToPlane(position);
+				var agentRadius = agentData.radius[agentIndex];
+
+				for (int neighbourIndex = 0; neighbourIndex < neighbours.Length; neighbourIndex++) {
+					int otherIndex = neighbours[neighbourIndex];
+					// Indicates that there are no more neighbours (see JobRVOCalculateNeighbours)
+					if (otherIndex == -1) break;
+
+					var otherPosition = agentData.position[otherIndex];
+					var relativePosition = movementPlane.ToPlane(otherPosition - position);
+					float combinedRadius = agentRadius + agentData.radius[otherIndex];
+
+					var otherPriority = agentData.priority[otherIndex] * priorityMultiplier;
+
+					// TODO: Remove branches to possibly vectorize
+					float avoidanceStrength;
+					if (agentData.locked[otherIndex] || agentData.manuallyControlled[otherIndex]) {
+						avoidanceStrength = 1;
+					} else if (otherPriority > 0.00001f || priority > 0.00001f) {
+						avoidanceStrength = otherPriority / (priority + otherPriority);
+					} else {
+						// Both this agent's priority and the other agent's priority is zero or negative
+						// Assume they have the same priority
+						avoidanceStrength = 0.5f;
+					}
+
+					// We assume that the other agent will continue to move with roughly the same velocity if the priorities for the agents are similar.
+					// If the other agent has a higher priority than this agent (avoidanceStrength > 0.5) then we will assume it will move more along its
+					// desired velocity. This will have the effect of other agents trying to clear a path for where a high priority agent wants to go.
+					// If this is not done then even high priority agents can get stuck when it is really crowded and they have had to slow down.
+					float2 otherOptimalVelocity = movementPlane.ToPlane(math.lerp(temporaryAgentData.currentVelocity[otherIndex], temporaryAgentData.desiredVelocity[otherIndex], math.clamp(2*avoidanceStrength - 1, 0, 1)));
+
+					if (agentData.flowFollowingStrength[otherIndex] > 0) {
+						// When flow following strength is 1 the component of the other agent's velocity that is in the direction of this agent is removed.
+						// That is, we pretend that the other agent does not move towards this agent at all.
+						// This will make it impossible for the other agent to "push" this agent away.
+						var strength = agentData.flowFollowingStrength[otherIndex] * agentData.flowFollowingStrength[agentIndex];
+						var relativeDir = math.normalizesafe(relativePosition);
+						otherOptimalVelocity -= relativeDir * (strength * math.min(0, math.dot(otherOptimalVelocity, relativeDir)));
+					}
+
+					var dist = math.length(relativePosition);
+					// Figure out an approximate time to collision. We avoid using the current velocities of the agents because that leads to oscillations,
+					// as the agents change their velocities, which results in a change to the time to collision, which makes them change their velocities again.
+					var minimumTimeToCollision = math.max(0, dist - combinedRadius) / math.max(combinedRadius, agentData.desiredSpeed[agentIndex] + agentData.desiredSpeed[otherIndex]);
+
+					// Adjust the radius to make the avoidance smoother.
+					// The agent will slowly start to take another agent into account instead of making a sharp turn.
+					float normalizedTime = minimumTimeToCollision * inverseAgentTimeHorizon;
+					// normalizedTime <= 0.5 => 0% effect
+					// normalizedTime  = 1.0 => 100% effect
+					var factor = math.clamp((normalizedTime - 0.5f)*2.0f, 0, 1);
+					combinedRadius *= 1 - factor;
+
+					// Adjust the time horizon to make the agent approach another agent less conservatively.
+					// This makes the velocity curve closer to sqrt(1-t) instead of exp(-t) as it comes to a stop, which looks nicer.
+					var tempInverseTimeHorizon = 1.0f/math.max(0.1f*agentTimeHorizon, agentTimeHorizon * math.clamp(math.sqrt(2f*minimumTimeToCollision), 0, 1));
+
+					orcaLines[numLines] = new ORCALine(localPosition, relativePosition, optimalVelocity, otherOptimalVelocity, combinedRadius, 0.1f, tempInverseTimeHorizon);
+					orcaLineToAgent[numLines] = otherIndex;
+					numLines++;
+#if UNITY_EDITOR
+					if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.AgentVOs)) {
+						draw.PushMatrix(math.mul(float4x4.TRS(position, quaternion.identity, 1), movementPlane.matrix));
+						var voCenter = math.lerp(optimalVelocity, otherOptimalVelocity, 0.5f);
+						DrawVO(draw, relativePosition * tempInverseTimeHorizon + otherOptimalVelocity, combinedRadius * tempInverseTimeHorizon, otherOptimalVelocity, Color.black);
+						draw.PopMatrix();
+					}
+#endif
+				}
+
+				// Add an obstacle for the collision normal.
+				// This is mostly deprecated, but kept for compatibility.
+				var collisionNormal = math.normalizesafe(movementPlane.ToPlane(agentData.collisionNormal[agentIndex]));
+				if (math.any(collisionNormal != 0)) {
+					orcaLines[numLines] = new ORCALine {
+						point = float2.zero,
+						direction = new float2(collisionNormal.y, -collisionNormal.x),
+					};
+					orcaLineToAgent[numLines] = -1;
+					numLines++;
+				}
+
+				var desiredVelocity = movementPlane.ToPlane(temporaryAgentData.desiredVelocity[agentIndex]);
+				var desiredTargetPointInVelocitySpace = temporaryAgentData.desiredTargetPointInVelocitySpace[agentIndex];
+				var originalDesiredVelocity = desiredVelocity;
+				var symmetryBias = symmetryBreakingBias * (1 - agentData.flowFollowingStrength[agentIndex]);
+				// Bias the desired velocity to avoid symmetry issues (esp. when two agents are heading straight towards one another).
+				// Do not bias velocities if the agent is heading towards an obstacle (not an agent).
+				bool insideAnyVO = BiasDesiredVelocity(orcaLines.AsUnsafeSpan().Slice(numFixedLines, numLines - numFixedLines), ref desiredVelocity, ref desiredTargetPointInVelocitySpace, symmetryBias);
+				// If the velocity is outside all agent orca half-planes, do a more thorough check of all orca lines (including obstacles).
+				insideAnyVO = insideAnyVO || DistanceInsideVOs(orcaLines.AsUnsafeSpan().Slice(0, numLines), desiredVelocity) > 0;
+
+
+#if UNITY_EDITOR
+				if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.ObstacleVOs)) {
+					draw.PushColor(new Color(1, 1, 1, 0.2f));
+					draw.PushMatrix(math.mul(float4x4.TRS(position, quaternion.identity, 1), movementPlane.matrix));
+					for (int i = 0; i < numLines; i++) {
+						orcaLines[i].DrawAsHalfPlane(draw, agentData.radius[agentIndex] * 5.0f, 1.0f, i >= numFixedLines ? Color.magenta : Color.Lerp(Color.magenta, Color.black, 0.5f));
+					}
+					draw.PopMatrix();
+					draw.PopColor();
+				}
+#endif
+
+				if (!insideAnyVO && math.all(math.abs(temporaryAgentData.collisionVelocityOffsets[agentIndex]) < 0.001f)) {
+					// Desired velocity can be used directly since it was not inside any velocity obstacle.
+					// No need to run optimizer because this will be the global minima.
+					// This is also a special case in which we can set the
+					// calculated target point to the desired target point
+					// instead of calculating a point based on a calculated velocity
+					// which is an important difference when the agent is very close
+					// to the target point
+					// TODO: Not actually guaranteed to be global minima if desiredTargetPointInVelocitySpace.magnitude < desiredSpeed
+					// maybe do something different here?
+#if UNITY_EDITOR
+					if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.DesiredVelocity)) {
+						draw.xy.Cross(movementPlane.ToWorld(localPosition + desiredVelocity), Color.magenta);
+						draw.xy.Cross(movementPlane.ToWorld(localPosition + desiredTargetPointInVelocitySpace), Color.yellow);
+					}
+#endif
+
+					output.targetPoint[agentIndex] = position + movementPlane.ToWorld(desiredTargetPointInVelocitySpace, 0);
+					output.speed[agentIndex] = agentData.desiredSpeed[agentIndex];
+					output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount] = -1;
+					output.forwardClearance[agentIndex] = float.PositiveInfinity;
+				} else {
+					var maxSpeed = agentData.maxSpeed[agentIndex];
+					var allowedVelocityDeviationAngles = agentData.allowedVelocityDeviationAngles[agentIndex];
+					LinearProgram2Output lin;
+					if (math.all(allowedVelocityDeviationAngles == 0)) {
+						// Common case, the desired velocity is a point
+						lin = LinearProgram2D(orcaLines, numLines, maxSpeed, desiredVelocity, false);
+					} else {
+						// The desired velocity is a segment, not a point
+
+						// Rotate the desired velocity allowedVelocityDeviationAngles.x radians and allowedVelocityDeviationAngles.y radians respectively
+						math.sincos(allowedVelocityDeviationAngles, out float2 s, out float2 c);
+						var xs = desiredVelocity.x*c - desiredVelocity.y*s;
+						var ys = desiredVelocity.x*s + desiredVelocity.y*c;
+						var desiredVelocityLeft = new float2(xs.x, ys.x);
+						var desiredVelocityRight = new float2(xs.y, ys.y);
+
+						var desiredVelocityLeftDir = desiredVelocity - desiredVelocityLeft;
+
+						// Normalize and store length
+						var desiredVelocityLeftSegmentLength = math.length(desiredVelocityLeftDir);
+						desiredVelocityLeftDir = math.select(float2.zero, desiredVelocityLeftDir * math.rcp(desiredVelocityLeftSegmentLength), desiredVelocityLeftSegmentLength > math.FLT_MIN_NORMAL);
+
+						var desiredVelocityRightDir = desiredVelocity - desiredVelocityRight;
+						var desiredVelocityRightSegmentLength = math.length(desiredVelocityRightDir);
+						desiredVelocityRightDir = math.select(float2.zero, desiredVelocityRightDir * math.rcp(desiredVelocityRightSegmentLength), desiredVelocityRightSegmentLength > math.FLT_MIN_NORMAL);
+
+						// var tOptimal = ClosestPointOnSegment(desiredVelocityLeft, desiredVelocityDir, desiredVelocity, 0, desiredVelocitySegmentLength);
+
+						var lin1 = LinearProgram2DSegment(orcaLines, numLines, maxSpeed, desiredVelocityLeft, desiredVelocityLeftDir, 0, desiredVelocityLeftSegmentLength, 1.0f);
+						var lin2 = LinearProgram2DSegment(orcaLines, numLines, maxSpeed, desiredVelocityRight, desiredVelocityRightDir, 0, desiredVelocityRightSegmentLength, 1.0f);
+
+						if (lin1.firstFailedLineIndex < lin2.firstFailedLineIndex) {
+							lin = lin1;
+						} else if (lin2.firstFailedLineIndex < lin1.firstFailedLineIndex) {
+							lin = lin2;
+						} else {
+							lin = math.lengthsq(lin1.velocity - desiredVelocity) < math.lengthsq(lin2.velocity - desiredVelocity) ? lin1 : lin2;
+						}
+					}
+
+					float2 newVelocity;
+					if (lin.firstFailedLineIndex < numLines) {
+						newVelocity = lin.velocity;
+						LinearProgram3D(orcaLines, numLines, numFixedLines, lin.firstFailedLineIndex, maxSpeed, ref newVelocity, scratchBuffer);
+					} else {
+						newVelocity = lin.velocity;
+					}
+
+#if UNITY_EDITOR
+					if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.ChosenVelocity)) {
+						draw.xy.Cross(position + movementPlane.ToWorld(newVelocity), Color.white);
+						draw.Arrow(position + movementPlane.ToWorld(desiredVelocity), position + movementPlane.ToWorld(newVelocity), Color.magenta);
+					}
+#endif
+
+					var blockedByAgentCount = 0;
+					for (int i = 0; i < numLines && blockedByAgentCount < SimulatorBurst.MaxBlockingAgentCount; i++) {
+						if (orcaLineToAgent[i] != -1 && det(orcaLines[i].direction, orcaLines[i].point - newVelocity) >= -0.001f) {
+							// We are blocked by this line
+							output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount + blockedByAgentCount] = orcaLineToAgent[i];
+							blockedByAgentCount++;
+						}
+					}
+					if (blockedByAgentCount < SimulatorBurst.MaxBlockingAgentCount) output.blockedByAgents[agentIndex*SimulatorBurst.MaxBlockingAgentCount + blockedByAgentCount] = -1;
+
+					var collisionVelocityOffset = temporaryAgentData.collisionVelocityOffsets[agentIndex];
+					if (math.any(collisionVelocityOffset != 0)) {
+						// Make the agent move to avoid intersecting other agents (hard collisions)
+						newVelocity += temporaryAgentData.collisionVelocityOffsets[agentIndex];
+
+						// Adding the collision offset may have made the velocity invalid, causing it to intersect the wall-velocity-obstacles.
+						// We run a second optimization on only the wall-velocity-obstacles to make sure the velocity is valid.
+						newVelocity = LinearProgram2D(orcaLines, numFixedLines, maxSpeed, newVelocity, false).velocity;
+					}
+
+					output.targetPoint[agentIndex] = position + movementPlane.ToWorld(newVelocity, 0);
+					output.speed[agentIndex] = math.min(math.length(newVelocity), maxSpeed);
+
+					var targetDir = math.normalizesafe(movementPlane.ToPlane(agentData.targetPoint[agentIndex] - position));
+					var forwardClearance = CalculateForwardClearance(neighbours, movementPlane, position, agentRadius, targetDir);
+					output.forwardClearance[agentIndex] = forwardClearance;
+					if (agentData.HasDebugFlag(agentIndex, AgentDebugFlags.ForwardClearance) && forwardClearance < float.PositiveInfinity) {
+						draw.PushLineWidth(2);
+						draw.Ray(position, movementPlane.ToWorld(targetDir) * forwardClearance, Color.red);
+						draw.PopLineWidth();
+					}
+				}
+			}
+		}
+
+		/// <summary>
+		/// Find the distance we can move towards our target without colliding with anything.
+		/// May become negative if we are currently colliding with something.
+		/// </summary>
+		float CalculateForwardClearance (NativeSlice<int> neighbours, MovementPlaneWrapper movementPlane, float3 position, float radius, float2 targetDir) {
+			// TODO: Take obstacles into account.
+			var smallestIntersectionDistance = float.PositiveInfinity;
+			for (int i = 0; i < neighbours.Length; i++) {
+				var other = neighbours[i];
+				if (other == -1) break;
+				var otherPosition = agentData.position[other];
+				var combinedRadius = radius + agentData.radius[other];
+				// Intersect the ray from our agent towards the destination and check the distance to the intersection with the other agent.
+				var otherDir = movementPlane.ToPlane(otherPosition - position);
+				// Squared cosine of the angle between otherDir and ourTargetDir
+				var cosAlpha = math.dot(math.normalizesafe(otherDir), targetDir);
+
+				// Check if the agent is behind us
+				if (cosAlpha < 0) continue;
+
+				var distToOtherSq = math.lengthsq(otherDir);
+				var distToClosestPointAlongRay = math.sqrt(distToOtherSq) * cosAlpha;
+				var discriminant = combinedRadius*combinedRadius - (distToOtherSq - distToClosestPointAlongRay*distToClosestPointAlongRay);
+
+				// Check if we have any intersection at all
+				if (discriminant < 0) continue;
+				var distToIntersection = distToClosestPointAlongRay - math.sqrt(discriminant);
+				smallestIntersectionDistance = math.min(smallestIntersectionDistance, distToIntersection);
+			}
+			return smallestIntersectionDistance;
+		}
+
+		/// <summary>True if vector2 is to the left of vector1 or if they are colinear.</summary>
+		static bool leftOrColinear (float2 vector1, float2 vector2) {
+			return det(vector1, vector2) >= 0;
+		}
+
+		/// <summary>True if vector2 is to the left of vector1.</summary>
+		static bool left (float2 vector1, float2 vector2) {
+			return det(vector1, vector2) > 0;
+		}
+
+		/// <summary>True if vector2 is to the right of vector1 or if they are colinear.</summary>
+		static bool rightOrColinear (float2 vector1, float2 vector2) {
+			return det(vector1, vector2) <= 0;
+		}
+
+		/// <summary>True if vector2 is to the right of vector1.</summary>
+		static bool right (float2 vector1, float2 vector2) {
+			return det(vector1, vector2) < 0;
+		}
+
+		/// <summary>
+		/// Determinant of the 2x2 matrix defined by vector1 and vector2.
+		/// Alternatively, the Z component of the cross product of vector1 and vector2.
+		/// </summary>
+		static float det (float2 vector1, float2 vector2) {
+			return vector1.x * vector2.y - vector1.y * vector2.x;
+		}
+
+		static float2 rot90 (float2 v) {
+			return new float2(-v.y, v.x);
+		}
+
+		/// <summary>
+		/// A half-plane defined as the line splitting plane.
+		///
+		/// For ORCA purposes, the infeasible region of the half-plane is on the right side of the line.
+		/// </summary>
+		struct ORCALine {
+			public float2 point;
+			public float2 direction;
+
+			public void DrawAsHalfPlane (CommandBuilder draw, float halfPlaneLength, float halfPlaneWidth, Color color) {
+				var normal = new float2(direction.y, -direction.x);
+				draw.xy.Line(point - direction*10, point + direction*10, color);
+
+				var p = point + normal*halfPlaneWidth*0.5f;
+				draw.SolidBox(new float3(p, 0), quaternion.RotateZ(math.atan2(direction.y, direction.x)), new float3(halfPlaneLength, halfPlaneWidth, 0.01f), new Color(0, 0, 0, 0.5f));
+			}
+
+			public ORCALine(float2 position, float2 relativePosition, float2 velocity, float2 otherVelocity, float combinedRadius, float timeStep, float invTimeHorizon) {
+				var relativeVelocity = velocity - otherVelocity;
+				float combinedRadiusSq = combinedRadius*combinedRadius;
+				float distSq = math.lengthsq(relativePosition);
+
+				if (distSq > combinedRadiusSq) {
+					combinedRadius *= 1.001f;
+					// No collision
+
+					// A velocity obstacle is built which is shaped like a truncated cone (see ORCA paper).
+					// The cone is truncated by an arc centered at relativePosition/timeHorizon
+					// with radius combinedRadius/timeHorizon.
+					// The cone extends in the direction of relativePosition.
+
+					// Vector from truncation arc center to relative velocity
+					var w = relativeVelocity - invTimeHorizon * relativePosition;
+					var wLengthSq = math.lengthsq(w);
+
+					float dot1 = math.dot(w, relativePosition);
+
+					if (dot1 < 0.0f && dot1*dot1 > combinedRadiusSq * wLengthSq) {
+						// Project on cut-off circle
+						float wLength = math.sqrt(wLengthSq);
+						var normalizedW = w / wLength;
+
+						direction = new float2(normalizedW.y, -normalizedW.x);
+						var u = (combinedRadius * invTimeHorizon - wLength) * normalizedW;
+						point = velocity + 0.5f * u;
+					} else {
+						// Project on legs
+						// Distance from the agent to the point where the "legs" start on the VO
+						float legDistance = math.sqrt(distSq - combinedRadiusSq);
+
+						if (det(relativePosition, w) > 0.0f) {
+							// Project on left leg
+							// Note: This vector is actually normalized
+							direction = (relativePosition * legDistance + new float2(-relativePosition.y, relativePosition.x) * combinedRadius) / distSq;
+						} else {
+							// Project on right leg
+							// Note: This vector is actually normalized
+							direction = (-relativePosition * legDistance + new float2(-relativePosition.y, relativePosition.x) * combinedRadius) / distSq;
+						}
+
+						float dot2 = math.dot(relativeVelocity, direction);
+						var u = dot2 * direction - relativeVelocity;
+						point = velocity + 0.5f * u;
+					}
+				} else {
+					float invTimeStep = math.rcp(timeStep);
+					var dist = math.sqrt(distSq);
+					var normalizedDir = math.select(0, relativePosition / dist, dist > math.FLT_MIN_NORMAL);
+					var u = normalizedDir * (dist - combinedRadius - 0.001f) * 0.3f * invTimeStep;
+					direction = math.normalizesafe(new float2(u.y, -u.x));
+					point = math.lerp(velocity, otherVelocity, 0.5f) + u * 0.5f;
+
+
+					// Original code, the above is a version which works better
+					// Collision
+					// Project on cut-off circle of timeStep
+					//float invTimeStep = 1.0f / timeStep;
+					// Vector from cutoff center to relative velocity
+					//float2 w = relativeVelocity - invTimeStep * relativePosition;
+					//float wLength = math.length(w);
+					//float2 unitW = w / wLength;
+					//direction = new float2(unitW.y, -unitW.x);
+					//var u = (combinedRadius * invTimeStep - wLength) * unitW;
+					//point = velocity + 0.5f * u;
+				}
+			}
+		}
+
+		/// <summary>
+		/// Calculates how far inside the infeasible region of the ORCA half-planes the velocity is.
+		/// Returns 0 if the velocity is in the feasible region of all half-planes.
+		/// </summary>
+		static float DistanceInsideVOs (UnsafeSpan<ORCALine> lines, float2 velocity) {
+			float maxDistance = 0.0f;
+
+			for (int i = 0; i < lines.Length; i++) {
+				var distance = det(lines[i].direction, lines[i].point - velocity);
+				maxDistance = math.max(maxDistance, distance);
+			}
+
+			return maxDistance;
+		}
+
+		/// <summary>
+		/// Bias towards the right side of agents.
+		/// Rotate desiredVelocity at most [value] number of radians. 1 radian ≈ 57°
+		/// This breaks up symmetries.
+		///
+		/// The desired velocity will only be rotated if it is inside a velocity obstacle (VO).
+		/// If it is inside one, it will not be rotated further than to the edge of it
+		///
+		/// The targetPointInVelocitySpace will be rotated by the same amount as the desired velocity
+		///
+		/// Returns: True if the desired velocity was inside any VO
+		/// </summary>
+		static bool BiasDesiredVelocity (UnsafeSpan<ORCALine> lines, ref float2 desiredVelocity, ref float2 targetPointInVelocitySpace, float maxBiasRadians) {
+			float maxDistance = DistanceInsideVOs(lines, desiredVelocity);
+
+			if (maxDistance == 0.0f) return false;
+
+			var desiredVelocityMagn = math.length(desiredVelocity);
+
+			// Avoid division by zero below
+			if (desiredVelocityMagn >= 0.001f) {
+				// Rotate the desired velocity clockwise (to the right) at most maxBiasRadians number of radians.
+				// We clamp the angle so that we do not rotate more than to the edge of the VO.
+				// Assuming maxBiasRadians is small, we can just move it instead and it will give approximately the same effect.
+				// See https://en.wikipedia.org/wiki/Small-angle_approximation
+				var angle = math.min(maxBiasRadians, maxDistance / desiredVelocityMagn);
+				desiredVelocity += new float2(desiredVelocity.y, -desiredVelocity.x) * angle;
+				targetPointInVelocitySpace += new float2(targetPointInVelocitySpace.y, -targetPointInVelocitySpace.x) * angle;
+			}
+			return true;
+		}
+
+		/// <summary>
+		/// Clip a line to the feasible region of the half-plane given by the clipper.
+		/// The clipped line is `line.point + line.direction*tLeft` to `line.point + line.direction*tRight`.
+		///
+		/// Returns false if the line is parallel to the clipper's border.
+		/// </summary>
+		static bool ClipLine (ORCALine line, ORCALine clipper, ref float tLeft, ref float tRight) {
+			float denominator = det(line.direction, clipper.direction);
+			float numerator = det(clipper.direction, line.point - clipper.point);
+
+			if (math.abs(denominator) < 0.0001f) {
+				// The two lines are almost parallel
+				return false;
+			}
+
+			float t = numerator / denominator;
+
+			if (denominator >= 0.0f) {
+				// Line i bounds the line on the right
+				tRight = math.min(tRight, t);
+			} else {
+				// Line i bounds the line on the left
+				tLeft = math.max(tLeft, t);
+			}
+			return true;
+		}
+
+		static bool ClipBoundary (NativeArray<ORCALine> lines, int lineIndex, float radius, out float tLeft, out float tRight) {
+			var line = lines[lineIndex];
+			if (!VectorMath.LineCircleIntersectionFactors(line.point, line.direction, radius, out tLeft, out tRight)) {
+				return false;
+			}
+
+			// Go through all previous lines/half-planes and clip the current line against them
+			for (int i = 0; i < lineIndex; i++) {
+				float denominator = det(line.direction, lines[i].direction);
+				float numerator = det(lines[i].direction, line.point - lines[i].point);
+
+				if (math.abs(denominator) < 0.0001f) {
+					// The two lines are almost parallel
+					if (numerator < 0.0f) {
+						// This line is completely "behind" the other line. So we can ignore it.
+						return false;
+					} else continue;
+				}
+
+				float t = numerator / denominator;
+
+				if (denominator >= 0.0f) {
+					// Line i bounds the line on the right
+					tRight = math.min(tRight, t);
+				} else {
+					// Line i bounds the line on the left
+					tLeft = math.max(tLeft, t);
+				}
+
+				if (tLeft > tRight) {
+					// The line is completely outside the previous half-planes
+					return false;
+				}
+			}
+			return true;
+		}
+
+		static bool LinearProgram1D (NativeArray<ORCALine> lines, int lineIndex, float radius, float2 optimalVelocity, bool directionOpt, ref float2 result) {
+			if (!ClipBoundary(lines, lineIndex, radius, out float tLeft, out float tRight)) return false;
+			var line = lines[lineIndex];
+
+			if (directionOpt) {
+				// Optimize direction
+				if (math.dot(optimalVelocity, line.direction) > 0.0f) {
+					// Take right extreme
+					result = line.point + tRight * line.direction;
+				} else {
+					// Take left extreme
+					result = line.point + tLeft * line.direction;
+				}
+			} else {
+				// Optimize closest point
+				float t = math.dot(line.direction, optimalVelocity - line.point);
+				result = line.point + math.clamp(t, tLeft, tRight) * line.direction;
+			}
+			return true;
+		}
+
+		struct LinearProgram2Output {
+			public float2 velocity;
+			public int firstFailedLineIndex;
+		}
+
+		static LinearProgram2Output LinearProgram2D (NativeArray<ORCALine> lines, int numLines, float radius, float2 optimalVelocity, bool directionOpt) {
+			float2 result;
+
+			if (directionOpt) {
+				// Optimize direction. Note that the optimization velocity is of unit length in this case
+				result = optimalVelocity * radius;
+			} else if (math.lengthsq(optimalVelocity) > radius*radius) {
+				// Optimize closest point and outside circle
+				result = math.normalize(optimalVelocity) * radius;
+			} else {
+				// Optimize closest point and inside circle
+				result = optimalVelocity;
+			}
+
+			for (int i = 0; i < numLines; i++) {
+				// Check if point is in the infeasible region of the half-plane
+				if (det(lines[i].direction, lines[i].point - result) > 0.0f) {
+					// Result does not satisfy constraint i. Compute new optimal result
+					var tempResult = result;
+					if (!LinearProgram1D(lines, i, radius, optimalVelocity, directionOpt, ref result)) {
+						return new LinearProgram2Output {
+								   velocity = tempResult,
+								   firstFailedLineIndex = i,
+						};
+					}
+				}
+			}
+
+			return new LinearProgram2Output {
+					   velocity = result,
+					   firstFailedLineIndex = numLines,
+			};
+		}
+
+		static float ClosestPointOnSegment (float2 a, float2 dir, float2 p, float t0, float t1) {
+			return math.clamp(math.dot(p - a, dir), t0, t1);
+		}
+
+		/// <summary>
+		/// Closest point on segment a to segment b.
+		/// The segments are given by infinite lines and bounded by t values. p = line.point + line.dir*t.
+		///
+		/// It is assumed that the two segments do not intersect.
+		/// </summary>
+		static float2 ClosestSegmentSegmentPointNonIntersecting (ORCALine a, ORCALine b, float ta1, float ta2, float tb1, float tb2) {
+			// We know that the two segments do not intersect, so at least one of the closest points
+			// must be one of the line segment endpoints.
+			var ap0 = a.point + a.direction*ta1;
+			var ap1 = a.point + a.direction*ta2;
+			var bp0 = b.point + b.direction * tb1;
+			var bp1 = b.point + b.direction * tb2;
+
+			var t0 = ClosestPointOnSegment(a.point, a.direction, bp0, ta1, ta2);
+			var t1 = ClosestPointOnSegment(a.point, a.direction, bp1, ta1, ta2);
+			var t2 = ClosestPointOnSegment(b.point, b.direction, ap0, tb1, tb2);
+			var t3 = ClosestPointOnSegment(b.point, b.direction, ap1, tb1, tb2);
+
+			var c0 = a.point + a.direction * t0;
+			var c1 = a.point + a.direction * t1;
+			var c2 = b.point + b.direction * t2;
+			var c3 = b.point + b.direction * t3;
+
+			var d0 = math.lengthsq(c0 - bp0);
+			var d1 = math.lengthsq(c1 - bp1);
+			var d2 = math.lengthsq(c2 - ap0);
+			var d3 = math.lengthsq(c3 - ap1);
+
+			var result = c0;
+			var d = d0;
+			if (d1 < d) {
+				result = c1;
+				d = d1;
+			}
+			if (d2 < d) {
+				result = ap0;
+				d = d2;
+			}
+			if (d3 < d) {
+				result = ap1;
+				d = d3;
+			}
+			return result;
+		}
+
+		/// <summary>Like LinearProgram2D, but the optimal velocity space is a segment instead of a point, however the current result has collapsed to a point</summary>
+		static LinearProgram2Output LinearProgram2DCollapsedSegment (NativeArray<ORCALine> lines, int numLines, int startLine, float radius, float2 currentResult, float2 optimalVelocityStart, float2 optimalVelocityDir, float optimalTLeft, float optimalTRight) {
+			for (int i = startLine; i < numLines; i++) {
+				// Check if point is in the infeasible region of the half-plane
+				if (det(lines[i].direction, lines[i].point - currentResult) > 0.0f) {
+					// Result does not satisfy constraint i. Compute new optimal result
+					if (!ClipBoundary(lines, i, radius, out float tLeft2, out float tRight2)) {
+						// We are partially not feasible, but no part of this constraint's boundary is in the feasible region.
+						// This means that there is no feasible solution at all.
+						return new LinearProgram2Output {
+								   velocity = currentResult,
+								   firstFailedLineIndex = i,
+						};
+					}
+
+					// Optimize closest point
+					currentResult = ClosestSegmentSegmentPointNonIntersecting(lines[i], new ORCALine {
+						point = optimalVelocityStart,
+						direction = optimalVelocityDir,
+					}, tLeft2, tRight2, optimalTLeft, optimalTRight);
+				}
+			}
+
+			return new LinearProgram2Output {
+					   velocity = currentResult,
+					   firstFailedLineIndex = numLines,
+			};
+		}
+
+		/// <summary>Like LinearProgram2D, but the optimal velocity space is a segment instead of a point</summary>
+		static LinearProgram2Output LinearProgram2DSegment (NativeArray<ORCALine> lines, int numLines, float radius, float2 optimalVelocityStart, float2 optimalVelocityDir, float optimalTLeft, float optimalTRight, float optimalT) {
+			var hasIntersection = VectorMath.LineCircleIntersectionFactors(optimalVelocityStart, optimalVelocityDir, radius, out float resultTLeft, out float resultTRight);
+			resultTLeft = math.max(resultTLeft, optimalTLeft);
+			resultTRight = math.min(resultTRight, optimalTRight);
+			hasIntersection &= resultTLeft <= resultTRight;
+
+			if (!hasIntersection) {
+				// In case the optimal velocity segment is not inside the max velocity circle, then collapse to a single optimal velocity which
+				// is closest segment point to the circle
+				var t = math.clamp(math.dot(-optimalVelocityStart, optimalVelocityDir), optimalTLeft, optimalTRight);
+				var closestOnCircle = math.normalizesafe(optimalVelocityStart + optimalVelocityDir * t) * radius;
+
+				// The best point is now a single point, not a segment.
+				// So we can fall back to simpler code.
+				return LinearProgram2DCollapsedSegment(lines, numLines, 0, radius, closestOnCircle, optimalVelocityStart, optimalVelocityDir, optimalTLeft, optimalTRight);
+			}
+
+			for (int i = 0; i < numLines; i++) {
+				// Check if optimal line segment is at least partially in the infeasible region of the half-plane
+				var line = lines[i];
+				var leftInfeasible = det(line.direction, line.point - (optimalVelocityStart + optimalVelocityDir*resultTLeft)) > 0.0f;
+				var rightInfeasible = det(line.direction, line.point - (optimalVelocityStart + optimalVelocityDir*resultTRight)) > 0.0f;
+				if (leftInfeasible || rightInfeasible) {
+					if (!ClipBoundary(lines, i, radius, out float tLeft, out float tRight)) {
+						// We are partially not feasible, but no part of this constraint's boundary is in the feasible region.
+						// This means that there is no feasible solution at all.
+						return new LinearProgram2Output {
+								   velocity = optimalVelocityStart + optimalVelocityDir * math.clamp(optimalT, resultTLeft, resultTRight),
+								   firstFailedLineIndex = i,
+						};
+					}
+
+					// Check if the optimal line segment is completely in the infeasible region
+					if (leftInfeasible && rightInfeasible) {
+						if (math.abs(det(line.direction, optimalVelocityDir)) < 0.001f) {
+							// Lines are almost parallel.
+							// Project the optimal velocity on the boundary
+							var t1 = ClosestPointOnSegment(line.point, line.direction, optimalVelocityStart + optimalVelocityDir*resultTLeft, tLeft, tRight);
+							var t2 = ClosestPointOnSegment(line.point, line.direction, optimalVelocityStart + optimalVelocityDir*resultTRight, tLeft, tRight);
+							var t3 = ClosestPointOnSegment(line.point, line.direction, optimalVelocityStart + optimalVelocityDir*optimalT, tLeft, tRight);
+							optimalVelocityStart = line.point;
+							optimalVelocityDir = line.direction;
+							resultTLeft = t1;
+							resultTRight = t2;
+							optimalT = t3;
+						} else {
+							// Find closest point on the constraint boundary segment to the optimal velocity segment
+							var result = ClosestSegmentSegmentPointNonIntersecting(line, new ORCALine {
+								point = optimalVelocityStart,
+								direction = optimalVelocityDir,
+							}, tLeft, tRight, optimalTLeft, optimalTRight);
+
+							// The best point is now a single point, not a segment.
+							// So we can fall back to simpler code.
+							return LinearProgram2DCollapsedSegment(lines, numLines, i+1, radius, result, optimalVelocityStart, optimalVelocityDir, optimalTLeft, optimalTRight);
+						}
+					} else {
+						// Clip optimal velocity segment to the constraint boundary.
+						// If this returns false and the lines are almost parallel, then we don't do anything
+						// because we already know they intersect. So the two lines must be almost identical.
+						ClipLine(new ORCALine {
+							point = optimalVelocityStart,
+							direction = optimalVelocityDir,
+						}, line, ref resultTLeft, ref resultTRight);
+					}
+				}
+			}
+
+			var resultT = math.clamp(optimalT, resultTLeft, resultTRight);
+
+			return new LinearProgram2Output {
+					   velocity = optimalVelocityStart + optimalVelocityDir * resultT,
+					   firstFailedLineIndex = numLines,
+			};
+		}
+
+		/// <summary>
+		/// Finds the velocity with the smallest maximum penetration into the given half-planes.
+		///
+		/// Assumes there are no points in the feasible region of the given half-planes.
+		///
+		/// Runs a 3-dimensional linear program, but projected down to 2D.
+		/// If there are no feasible regions outside all half-planes then we want to find the velocity
+		/// for which the maximum penetration into infeasible regions is minimized.
+		/// Conceptually we can solve this by taking our half-planes, and moving them outwards at a fixed speed
+		/// until there is exactly 1 feasible point.
+		/// We can formulate this in 3D space by thinking of the half-planes in 3D (velocity.x, velocity.y, penetration-depth) space, as sloped planes.
+		/// Moving the planes outwards then corresponds to decreasing the z coordinate.
+		/// In 3D space we want to find the point above all planes with the lowest z coordinate.
+		/// We do this by going through each plane and testing if it is possible that this plane
+		/// is the one with the maximum penetration.
+		/// If so, we know that the point will lie on the portion of that plane bounded by the intersections
+		/// with the other planes. We generate projected half-planes which represent the intersections with the
+		/// other 3D planes, and then we run a new optimization to find the point which penetrates this
+		/// half-plane the least.
+		/// </summary>
+		/// <param name="lines">The half-planes of all obstacles and agents.</param>
+		/// <param name="numLines">The number of half-planes in lines.</param>
+		/// <param name="numFixedLines">The number of half-planes in lines which are fixed (0..numFixedLines). These will be treated as static obstacles which should be avoided at all costs.</param>
+		/// <param name="beginLine">The index of the first half-plane in lines for which the previous optimization failed (see \reflink{LinearProgram2Output.firstFailedLineIndex}).</param>
+		/// <param name="radius">Maximum possible speed. This represents a circular velocity obstacle.</param>
+		/// <param name="result">Input is best velocity as output by \reflink{LinearProgram2D}. Output is the new best velocity. The velocity with the smallest maximum penetration into the given half-planes.</param>
+		/// <param name="scratchBuffer">A buffer of length at least numLines to use for scratch space.</param>
+		static void LinearProgram3D (NativeArray<ORCALine> lines, int numLines, int numFixedLines, int beginLine, float radius, ref float2 result, NativeArray<ORCALine> scratchBuffer) {
+			float distance = 0.0f;
+
+			NativeArray<ORCALine> projectedLines = scratchBuffer;
+			NativeArray<ORCALine>.Copy(lines, projectedLines, numFixedLines);
+
+			for (int i = beginLine; i < numLines; i++) {
+				// Check if #result is more than #distance units inside the infeasible region of the half-plane
+				if (det(lines[i].direction, lines[i].point - result) > distance) {
+					int numProjectedLines = numFixedLines;
+					for (int j = numFixedLines; j < i; j++) {
+						float determinant = det(lines[i].direction, lines[j].direction);
+						if (math.abs(determinant) < 0.001f) {
+							// Lines i and j are parallel
+							if (math.dot(lines[i].direction, lines[j].direction) > 0.0f) {
+								// Line i and j point in the same direction
+								continue;
+							} else {
+								// Line i and j point in the opposite direction
+								projectedLines[numProjectedLines] = new ORCALine {
+									point = 0.5f * (lines[i].point + lines[j].point),
+									direction = math.normalize(lines[j].direction - lines[i].direction),
+								};
+								numProjectedLines++;
+							}
+						} else {
+							projectedLines[numProjectedLines] = new ORCALine {
+								// The intersection between the two lines
+								point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction,
+								// The direction along which the intersection of the two 3D-planes intersect (projected onto the XY plane)
+								direction = math.normalize(lines[j].direction - lines[i].direction),
+							};
+							numProjectedLines++;
+						}
+					}
+
+					var lin = LinearProgram2D(projectedLines, numProjectedLines, radius, new float2(-lines[i].direction.y, lines[i].direction.x), true);
+					if (lin.firstFailedLineIndex < numProjectedLines) {
+						// This should in principle not happen.  The result is by definition
+						// already in the feasible region of this linear program. If it fails,
+						// it is due to small floating point error, and the current result is
+						// kept.
+					} else {
+						result = lin.velocity;
+					}
+
+					distance = det(lines[i].direction, lines[i].point - result);
+				}
+			}
+		}
+
+		static void DrawVO (CommandBuilder draw, float2 circleCenter, float radius, float2 origin, Color color) {
+#if UNITY_EDITOR
+			draw.PushColor(color);
+			float alpha = math.atan2((origin - circleCenter).y, (origin - circleCenter).x);
+			float gamma = radius/math.length(origin-circleCenter);
+			float delta = gamma <= 1.0f ? math.abs(math.acos(gamma)) : 0;
+
+			draw.xy.Circle(circleCenter, radius, alpha-delta, alpha+delta);
+			float2 p1 = new float2(math.cos(alpha-delta), math.sin(alpha-delta)) * radius;
+			float2 p2 = new float2(math.cos(alpha+delta), math.sin(alpha+delta)) * radius;
+
+			float2 p1t = -new float2(-p1.y, p1.x);
+			float2 p2t = new float2(-p2.y, p2.x);
+			p1 += circleCenter;
+			p2 += circleCenter;
+
+			draw.xy.Ray(p1, math.normalizesafe(p1t)*100);
+			draw.xy.Ray(p2, math.normalizesafe(p2t)*100);
+			draw.PopColor();
+#endif
+		}
+	}
+}
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgentBurst.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgentBurst.cs.meta
new file mode 100644
index 0000000..1e6a7e3
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgentBurst.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 87a0a74f7df9c401eaeb12dab0863446
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreObstacle.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreObstacle.cs
new file mode 100644
index 0000000..3b6ed49
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreObstacle.cs
@@ -0,0 +1,4 @@
+
+// This file has been removed from the project. Since UnityPackages cannot
+// delete files, only replace them, this message is left here to prevent old
+// files from causing compiler errors
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreObstacle.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreObstacle.cs.meta
new file mode 100644
index 0000000..0811a7a
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreObstacle.cs.meta
@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: 19c43d572022a4278a4d426f536b5ee4
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulator.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulator.cs
new file mode 100644
index 0000000..3b6ed49
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulator.cs
@@ -0,0 +1,4 @@
+
+// This file has been removed from the project. Since UnityPackages cannot
+// delete files, only replace them, this message is left here to prevent old
+// files from causing compiler errors
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulator.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulator.cs.meta
new file mode 100644
index 0000000..d9a2b4a
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulator.cs.meta
@@ -0,0 +1,7 @@
+fileFormatVersion: 2
+guid: f373cccc6991444b0b8b6e8c512842db
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulatorBurst.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulatorBurst.cs
new file mode 100644
index 0000000..bd93893
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulatorBurst.cs
@@ -0,0 +1,1226 @@
+using UnityEngine;
+using System.Collections.Generic;
+
+using Unity.Burst;
+using Unity.Jobs;
+using Unity.Mathematics;
+using Unity.Collections;
+
+/// <summary>Local avoidance related classes</summary>
+namespace Pathfinding.RVO {
+	using System;
+	using Pathfinding.Jobs;
+	using Pathfinding.Drawing;
+	using Pathfinding.Util;
+	using Pathfinding.ECS.RVO;
+
+	public interface IMovementPlaneWrapper {
+		float2 ToPlane(float3 p);
+		float2 ToPlane(float3 p, out float elevation);
+		float3 ToWorld(float2 p, float elevation = 0);
+		Bounds ToWorld(Bounds bounds);
+
+		/// <summary>Maps from 2D (X, Y, 0) coordinates to world coordinates</summary>
+		float4x4 matrix { get; }
+		void Set(NativeMovementPlane plane);
+	}
+
+	public struct XYMovementPlane : IMovementPlaneWrapper {
+		public float2 ToPlane(float3 p) => p.xy;
+		public float2 ToPlane (float3 p, out float elevation) {
+			elevation = p.z;
+			return p.xy;
+		}
+		public float3 ToWorld(float2 p, float elevation = 0) => new float3(p.x, p.y, elevation);
+		public Bounds ToWorld (Bounds bounds) {
+			var center = bounds.center;
+			var size = bounds.size;
+			return new Bounds(new Vector3(center.x, center.z, center.y), new Vector3(size.x, size.z, size.y));
+		}
+
+		public float4x4 matrix {
+			get {
+				return float4x4.identity;
+			}
+		}
+		public void Set (NativeMovementPlane plane) { }
+	}
+
+	public struct XZMovementPlane : IMovementPlaneWrapper {
+		public float2 ToPlane(float3 p) => p.xz;
+		public float2 ToPlane (float3 p, out float elevation) {
+			elevation = p.y;
+			return p.xz;
+		}
+		public float3 ToWorld(float2 p, float elevation = 0) => new float3(p.x, elevation, p.y);
+		public Bounds ToWorld(Bounds bounds) => bounds;
+		public void Set (NativeMovementPlane plane) { }
+		public float4x4 matrix => float4x4.RotateX(math.radians(90));
+	}
+
+	public struct ArbitraryMovementPlane : IMovementPlaneWrapper {
+		NativeMovementPlane plane;
+
+		public float2 ToPlane(float3 p) => plane.ToPlane(p);
+		public float2 ToPlane(float3 p, out float elevation) => plane.ToPlane(p, out elevation);
+		public float3 ToWorld(float2 p, float elevation = 0) => plane.ToWorld(p, elevation);
+		public Bounds ToWorld(Bounds bounds) => plane.ToWorld(bounds);
+		public void Set (NativeMovementPlane plane) {
+			this.plane = plane;
+		}
+		public float4x4 matrix {
+			get {
+				return math.mul(float4x4.TRS(0, plane.rotation, 1), new float4x4(
+					new float4(1, 0, 0, 0),
+					new float4(0, 0, 1, 0),
+					new float4(0, 1, 0, 0),
+					new float4(0, 0, 0, 1)
+					));
+			}
+		}
+	}
+
+	public struct IReadOnlySlice<T> : System.Collections.Generic.IReadOnlyList<T> {
+		public T[] data;
+		public int length;
+
+		public T this[int index] => data[index];
+
+		public int Count => length;
+
+		public IEnumerator<T> GetEnumerator () {
+			throw new System.NotImplementedException();
+		}
+
+		System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator () {
+			throw new System.NotImplementedException();
+		}
+	}
+
+	[System.Flags]
+	public enum AgentDebugFlags : byte {
+		Nothing = 0,
+		ObstacleVOs = 1 << 0,
+		AgentVOs = 1 << 1,
+		ReachedState = 1 << 2,
+		DesiredVelocity = 1 << 3,
+		ChosenVelocity = 1 << 4,
+		Obstacles = 1 << 5,
+		ForwardClearance = 1 << 6,
+	}
+
+	/// <summary>
+	/// Exposes properties of an Agent class.
+	///
+	/// See: RVOController
+	/// See: RVOSimulator
+	/// </summary>
+	public interface IAgent {
+		/// <summary>
+		/// Internal index of the agent.
+		/// See: <see cref="Pathfinding.RVO.SimulatorBurst.simulationData"/>
+		/// </summary>
+		int AgentIndex { get; }
+
+		/// <summary>
+		/// Position of the agent.
+		/// The agent does not move by itself, a movement script has to be responsible for
+		/// reading the CalculatedTargetPoint and CalculatedSpeed properties and move towards that point with that speed.
+		/// This property should ideally be set every frame.
+		/// </summary>
+		Vector3 Position { get; set; }
+
+		/// <summary>
+		/// Optimal point to move towards to avoid collisions.
+		/// The movement script should move towards this point with a speed of <see cref="CalculatedSpeed"/>.
+		///
+		/// See: RVOController.CalculateMovementDelta.
+		/// </summary>
+		Vector3 CalculatedTargetPoint { get; }
+
+		/// <summary>
+		/// True if the agent's movement is affected by any other agents or obstacles.
+		///
+		/// If the agent is all alone, and can just move in a straight line to its target, this will be false.
+		/// If it has to adjust its velocity, even slightly, to avoid collisions, this will be true.
+		/// </summary>
+		bool AvoidingAnyAgents { get; }
+
+		/// <summary>
+		/// Optimal speed of the agent to avoid collisions.
+		/// The movement script should move towards <see cref="CalculatedTargetPoint"/> with this speed.
+		/// </summary>
+		float CalculatedSpeed { get; }
+
+		/// <summary>
+		/// Point towards which the agent should move.
+		/// Usually you set this once per frame. The agent will try move as close to the target point as possible.
+		/// Will take effect at the next simulation step.
+		///
+		/// Note: The system assumes that the agent will stop when it reaches the target point
+		/// so if you just want to move the agent in a particular direction, make sure that you set the target point
+		/// a good distance in front of the character as otherwise the system may not avoid colisions that well.
+		/// What would happen is that the system (in simplified terms) would think that the agents would stop
+		/// before the collision and thus it wouldn't slow down or change course. See the image below.
+		/// In the image the desiredSpeed is the length of the blue arrow and the target point
+		/// is the point where the black arrows point to.
+		/// In the upper case the agent does not avoid the red agent (you can assume that the red
+		/// agent has a very small velocity for simplicity) while in the lower case it does.
+		/// If you are following a path a good way to pick the target point is to set it to
+		/// <code>
+		/// targetPoint = directionToNextWaypoint.normalized * remainingPathDistance
+		/// </code>
+		/// Where remainingPathDistance is the distance until the character would reach the end of the path.
+		/// This works well because at the end of the path the direction to the next waypoint will just be the
+		/// direction to the last point on the path and remainingPathDistance will be the distance to the last point
+		/// in the path, so targetPoint will be set to simply the last point in the path. However when remainingPathDistance
+		/// is large the target point will be so far away that the agent will essentially be told to move in a particular
+		/// direction, which is precisely what we want.
+		/// [Open online documentation to see images]
+		/// </summary>
+		/// <param name="targetPoint">Target point in world space.</param>
+		/// <param name="desiredSpeed">Desired speed of the agent. In world units per second. The agent will try to move with this
+		///      speed if possible.</param>
+		/// <param name="maxSpeed">Max speed of the agent. In world units per second. If necessary (for example if another agent
+		///      is on a collision trajectory towards this agent) the agent can move at this speed.
+		///      Should be at least as high as desiredSpeed, but it is recommended to use a slightly
+		///      higher value than desiredSpeed (for example desiredSpeed*1.2).</param>
+		/// <param name="endOfPath">Point in world space which is the agent's final desired destination on the navmesh.
+		/// 	This is typically the end of the path the agent is following.
+		/// 	May be set to (+inf,+inf,+inf) to mark the agent as not having a well defined end of path.
+		/// 	If this is set, multiple agents with roughly the same end of path will crowd more naturally around this point.
+		/// 	They will be able to realize that they cannot get closer if there are many agents trying to get closer to the same destination and then stop.</param>
+		void SetTarget(Vector3 targetPoint, float desiredSpeed, float maxSpeed, Vector3 endOfPath);
+
+		/// <summary>
+		/// Plane in which the agent moves.
+		/// Local avoidance calculations are always done in 2D and this plane determines how to convert from 3D to 2D.
+		///
+		/// In a typical 3D game the agents move in the XZ plane and in a 2D game they move in the XY plane.
+		/// By default this is set to the XZ plane.
+		///
+		/// See: <see cref="Pathfinding.Util.GraphTransform.xyPlane"/>
+		/// See: <see cref="Pathfinding.Util.GraphTransform.xzPlane"/>
+		/// </summary>
+		Util.SimpleMovementPlane MovementPlane { get; set; }
+
+		/// <summary>Locked agents will be assumed not to move</summary>
+		bool Locked { get; set; }
+
+		/// <summary>
+		/// Radius of the agent in world units.
+		/// Agents are modelled as circles/cylinders.
+		/// </summary>
+		float Radius { get; set; }
+
+		/// <summary>
+		/// Height of the agent in world units.
+		/// Agents are modelled as circles/cylinders.
+		/// </summary>
+		float Height { get; set; }
+
+		/// <summary>
+		/// Max number of estimated seconds to look into the future for collisions with agents.
+		/// As it turns out, this variable is also very good for controling agent avoidance priorities.
+		/// Agents with lower values will avoid other agents less and thus you can make 'high priority agents' by
+		/// giving them a lower value.
+		/// </summary>
+		float AgentTimeHorizon { get; set; }
+
+		/// <summary>Max number of estimated seconds to look into the future for collisions with obstacles</summary>
+		float ObstacleTimeHorizon { get; set; }
+
+		/// <summary>
+		/// Max number of agents to take into account.
+		/// Decreasing this value can lead to better performance, increasing it can lead to better quality of the simulation.
+		/// </summary>
+		int MaxNeighbours { get; set; }
+
+		/// <summary>Number of neighbours that the agent took into account during the last simulation step</summary>
+		int NeighbourCount { get; }
+
+		/// <summary>
+		/// Specifies the avoidance layer for this agent.
+		/// The <see cref="CollidesWith"/> mask on other agents will determine if they will avoid this agent.
+		/// </summary>
+		RVOLayer Layer { get; set; }
+
+		/// <summary>
+		/// Layer mask specifying which layers this agent will avoid.
+		/// You can set it as CollidesWith = RVOLayer.DefaultAgent | RVOLayer.Layer3 | RVOLayer.Layer6 ...
+		///
+		/// See: http://en.wikipedia.org/wiki/Mask_(computing)
+		/// See: bitmasks (view in online documentation for working links)
+		/// </summary>
+		RVOLayer CollidesWith { get; set; }
+
+		/// <summary>
+		/// Determines how strongly this agent just follows the flow instead of making other agents avoid it.
+		/// The default value is 0, if it is greater than zero (up to the maximum value of 1) other agents will
+		/// not avoid this character as much. However it works in a different way to <see cref="Priority"/>.
+		///
+		/// A group of agents with FlowFollowingStrength set to a high value that all try to reach the same point
+		/// will end up just settling to stationary positions around that point, none will push the others away to any significant extent.
+		/// This is tricky to achieve with priorities as priorities are all relative, so setting all agents to a low priority is the same thing
+		/// as not changing priorities at all.
+		///
+		/// Should be a value in the range [0, 1].
+		///
+		/// TODO: Add video
+		/// </summary>
+		float FlowFollowingStrength { get; set; }
+
+		/// <summary>Draw debug information in the scene view</summary>
+		AgentDebugFlags DebugFlags { get; set; }
+
+		/// <summary>
+		/// How strongly other agents will avoid this agent.
+		/// Usually a value between 0 and 1.
+		/// Agents with similar priorities will avoid each other with an equal strength.
+		/// If an agent sees another agent with a higher priority than itself it will avoid that agent more strongly.
+		/// In the extreme case (e.g this agent has a priority of 0 and the other agent has a priority of 1) it will treat the other agent as being a moving obstacle.
+		/// Similarly if an agent sees another agent with a lower priority than itself it will avoid that agent less.
+		///
+		/// In general the avoidance strength for this agent is:
+		/// <code>
+		/// if this.priority > 0 or other.priority > 0:
+		///     avoidanceStrength = other.priority / (this.priority + other.priority);
+		/// else:
+		///     avoidanceStrength = 0.5
+		/// </code>
+		/// </summary>
+		float Priority { get; set; }
+
+		int HierarchicalNodeIndex { get; set; }
+
+		/// <summary>
+		/// Callback which will be called right before avoidance calculations are started.
+		/// Used to update the other properties with the most up to date values
+		/// </summary>
+		System.Action PreCalculationCallback { set; }
+
+		/// <summary>
+		/// Callback which will be called right the agent is removed from the simulation.
+		/// This agent should not be used anymore after this callback has been called.
+		/// </summary>
+		System.Action DestroyedCallback { set; }
+
+		/// <summary>
+		/// Set the normal of a wall (or something else) the agent is currently colliding with.
+		/// This is used to make the RVO system aware of things like physics or an agent being clamped to the navmesh.
+		/// The velocity of this agent that other agents observe will be modified so that there is no component
+		/// into the wall. The agent will however not start to avoid the wall, for that you will need to add RVO obstacles.
+		///
+		/// This value will be cleared after the next simulation step, normally it should be set every frame
+		/// when the collision is still happening.
+		/// </summary>
+		void SetCollisionNormal(Vector3 normal);
+
+		/// <summary>
+		/// Set the current velocity of the agent.
+		/// This will override the local avoidance input completely.
+		/// It is useful if you have a player controlled character and want other agents to avoid it.
+		///
+		/// Calling this method will mark the agent as being externally controlled for 1 simulation step.
+		/// Local avoidance calculations will be skipped for the next simulation step but will be resumed
+		/// after that unless this method is called again.
+		/// </summary>
+		void ForceSetVelocity(Vector3 velocity);
+
+		public ReachedEndOfPath CalculatedEffectivelyReachedDestination { get; }
+
+		/// <summary>
+		/// Add obstacles to avoid for this agent.
+		///
+		/// The obstacles are based on nearby borders of the navmesh.
+		/// You should call this method every frame.
+		/// </summary>
+		/// <param name="sourceNode">The node to start the obstacle search at. This is typically the node the agent is standing on.</param>
+		public void SetObstacleQuery(GraphNode sourceNode);
+	}
+
+	/// <summary>
+	/// Type of obstacle shape.
+	/// See: <see cref="ObstacleVertexGroup"/>
+	/// </summary>
+	public enum ObstacleType {
+		/// <summary>A chain of vertices, the first and last segments end at a point</summary>
+		Chain,
+		/// <summary>A loop of vertices, the last vertex connects back to the first one</summary>
+		Loop,
+	}
+
+	public struct ObstacleVertexGroup {
+		/// <summary>Type of obstacle shape</summary>
+		public ObstacleType type;
+		/// <summary>Number of vertices that this group consists of</summary>
+		public int vertexCount;
+		public float3 boundsMn;
+		public float3 boundsMx;
+	}
+
+	/// <summary>Represents a set of obstacles</summary>
+	public struct UnmanagedObstacle {
+		/// <summary>The allocation in <see cref="ObstacleData.obstacleVertices"/> which represents all vertices used for these obstacles</summary>
+		public int verticesAllocation;
+		/// <summary>The allocation in <see cref="ObstacleData.obstacles"/> which represents the obstacle groups</summary>
+		public int groupsAllocation;
+	}
+
+	// TODO: Change to byte?
+	public enum ReachedEndOfPath {
+		/// <summary>The agent has no reached the end of its path yet</summary>
+		NotReached,
+		/// <summary>
+		/// The agent will soon reached the end of the path, or be blocked by other agents such that it cannot get closer.
+		/// Typically the agent can only move forward for a fraction of a second before it will become blocked.
+		/// </summary>
+		ReachedSoon,
+		/// <summary>
+		/// The agent has reached the end of the path, or it is blocked by other agents such that it cannot get closer right now.
+		/// If multiple have roughly the same end of path they will end up crowding around that point and all agents in the crowd will get this status.
+		/// </summary>
+		Reached,
+	}
+
+	// TODO: Change to byte?
+	/// <summary>Plane which movement is primarily happening in</summary>
+	public enum MovementPlane {
+		/// <summary>Movement happens primarily in the XZ plane (3D)</summary>
+		XZ,
+		/// <summary>Movement happens primarily in the XY plane (2D)</summary>
+		XY,
+		/// <summary>For curved worlds. See: spherical (view in online documentation for working links)</summary>
+		Arbitrary,
+	}
+
+	// Note: RVOLayer must not be marked with the [System.Flags] attribute because then Unity will show all RVOLayer fields as mask fields
+	// which we do not want
+	public enum RVOLayer {
+		DefaultAgent = 1 << 0,
+		DefaultObstacle = 1 << 1,
+		Layer2 = 1 << 2,
+		Layer3 = 1 << 3,
+		Layer4 = 1 << 4,
+		Layer5 = 1 << 5,
+		Layer6 = 1 << 6,
+		Layer7 = 1 << 7,
+		Layer8 = 1 << 8,
+		Layer9 = 1 << 9,
+		Layer10 = 1 << 10,
+		Layer11 = 1 << 11,
+		Layer12 = 1 << 12,
+		Layer13 = 1 << 13,
+		Layer14 = 1 << 14,
+		Layer15 = 1 << 15,
+		Layer16 = 1 << 16,
+		Layer17 = 1 << 17,
+		Layer18 = 1 << 18,
+		Layer19 = 1 << 19,
+		Layer20 = 1 << 20,
+		Layer21 = 1 << 21,
+		Layer22 = 1 << 22,
+		Layer23 = 1 << 23,
+		Layer24 = 1 << 24,
+		Layer25 = 1 << 25,
+		Layer26 = 1 << 26,
+		Layer27 = 1 << 27,
+		Layer28 = 1 << 28,
+		Layer29 = 1 << 29,
+		Layer30 = 1 << 30
+	}
+
+	/// <summary>
+	/// Local Avoidance Simulator.
+	/// This class handles local avoidance simulation for a number of agents using
+	/// Reciprocal Velocity Obstacles (RVO) and Optimal Reciprocal Collision Avoidance (ORCA).
+	///
+	/// This class will handle calculation of velocities from desired velocities supplied by a script.
+	/// It is, however, not responsible for moving any objects in a Unity Scene. For that there are other scripts (see below).
+	///
+	/// Agents be added and removed at any time.
+	///
+	/// See: RVOSimulator
+	/// See: RVOAgentBurst
+	/// See: Pathfinding.RVO.IAgent
+	///
+	/// You will most likely mostly use the wrapper class <see cref="RVOSimulator"/>.
+	/// </summary>
+	public class SimulatorBurst {
+		/// <summary>
+		/// Inverse desired simulation fps.
+		/// See: DesiredDeltaTime
+		/// </summary>
+		private float desiredDeltaTime = 0.05f;
+
+		/// <summary>Number of agents in this simulation</summary>
+		int numAgents = 0;
+
+		/// <summary>
+		/// Scope for drawing gizmos even on frames during which the simulation is not running.
+		/// This is used to draw the obstacles, quadtree and agent debug lines.
+		/// </summary>
+		Drawing.RedrawScope debugDrawingScope;
+
+		/// <summary>
+		/// Quadtree for this simulation.
+		/// Used internally by the simulation to perform fast neighbour lookups for each agent.
+		/// Please only read from this tree, do not rebuild it since that can interfere with the simulation.
+		/// It is rebuilt when necessary.
+		/// </summary>
+		public RVOQuadtreeBurst quadtree;
+
+		public bool drawQuadtree;
+
+		Action[] agentPreCalculationCallbacks = new Action[0];
+		Action[] agentDestroyCallbacks = new Action[0];
+
+		Stack<int> freeAgentIndices = new Stack<int>();
+		TemporaryAgentData temporaryAgentData;
+		HorizonAgentData horizonAgentData;
+
+		/// <summary>
+		/// Internal obstacle data.
+		/// Normally you will never need to access this directly
+		/// </summary>
+		public ObstacleData obstacleData;
+
+		/// <summary>
+		/// Internal simulation data.
+		/// Can be used if you need very high performance access to the agent data.
+		/// Normally you would use the SimulatorBurst.Agent class instead (implements the IAgent interface).
+		/// </summary>
+		public AgentData simulationData;
+
+		/// <summary>
+		/// Internal simulation data.
+		/// Can be used if you need very high performance access to the agent data.
+		/// Normally you would use the SimulatorBurst.Agent class instead (implements the IAgent interface).
+		/// </summary>
+		public AgentOutputData outputData;
+
+		public const int MaxNeighbourCount = 50;
+		public const int MaxBlockingAgentCount = 7;
+
+		public const int MaxObstacleVertices = 256;
+
+		struct Agent : IAgent {
+			public SimulatorBurst simulator;
+			public AgentIndex agentIndex;
+
+			public int AgentIndex => agentIndex.Index;
+			public Vector3 Position { get => simulator.simulationData.position[AgentIndex]; set => simulator.simulationData.position[AgentIndex] = value; }
+			public bool Locked { get => simulator.simulationData.locked[AgentIndex]; set => simulator.simulationData.locked[AgentIndex] = value; }
+			public float Radius { get => simulator.simulationData.radius[AgentIndex]; set => simulator.simulationData.radius[AgentIndex] = value; }
+			public float Height { get => simulator.simulationData.height[AgentIndex]; set => simulator.simulationData.height[AgentIndex] = value; }
+			public float AgentTimeHorizon { get => simulator.simulationData.agentTimeHorizon[AgentIndex]; set => simulator.simulationData.agentTimeHorizon[AgentIndex] = value; }
+			public float ObstacleTimeHorizon { get => simulator.simulationData.obstacleTimeHorizon[AgentIndex]; set => simulator.simulationData.obstacleTimeHorizon[AgentIndex] = value; }
+			public int MaxNeighbours { get => simulator.simulationData.maxNeighbours[AgentIndex]; set => simulator.simulationData.maxNeighbours[AgentIndex] = value; }
+			public RVOLayer Layer { get => simulator.simulationData.layer[AgentIndex]; set => simulator.simulationData.layer[AgentIndex] = value; }
+			public RVOLayer CollidesWith { get => simulator.simulationData.collidesWith[AgentIndex]; set => simulator.simulationData.collidesWith[AgentIndex] = value; }
+			public float FlowFollowingStrength { get => simulator.simulationData.flowFollowingStrength[AgentIndex]; set => simulator.simulationData.flowFollowingStrength[AgentIndex] = value; }
+			public AgentDebugFlags DebugFlags { get => simulator.simulationData.debugFlags[AgentIndex]; set => simulator.simulationData.debugFlags[AgentIndex] = value; }
+			public float Priority { get => simulator.simulationData.priority[AgentIndex]; set => simulator.simulationData.priority[AgentIndex] = value; }
+			public int HierarchicalNodeIndex { get => simulator.simulationData.hierarchicalNodeIndex[AgentIndex]; set => simulator.simulationData.hierarchicalNodeIndex[AgentIndex] = value; }
+			public SimpleMovementPlane MovementPlane { get => new SimpleMovementPlane(simulator.simulationData.movementPlane[AgentIndex].rotation); set => simulator.simulationData.movementPlane[AgentIndex] = new NativeMovementPlane(value); }
+			public Action PreCalculationCallback { set => simulator.agentPreCalculationCallbacks[AgentIndex] = value; }
+			public Action DestroyedCallback { set => simulator.agentDestroyCallbacks[AgentIndex] = value; }
+
+			public Vector3 CalculatedTargetPoint {
+				get {
+					simulator.BlockUntilSimulationStepDone();
+					return simulator.outputData.targetPoint[AgentIndex];
+				}
+			}
+
+			public float CalculatedSpeed {
+				get {
+					simulator.BlockUntilSimulationStepDone();
+					return simulator.outputData.speed[AgentIndex];
+				}
+			}
+
+			public ReachedEndOfPath CalculatedEffectivelyReachedDestination {
+				get {
+					simulator.BlockUntilSimulationStepDone();
+					return simulator.outputData.effectivelyReachedDestination[AgentIndex];
+				}
+			}
+
+			public int NeighbourCount {
+				get {
+					simulator.BlockUntilSimulationStepDone();
+					return simulator.outputData.numNeighbours[AgentIndex];
+				}
+			}
+
+			public bool AvoidingAnyAgents {
+				get {
+					simulator.BlockUntilSimulationStepDone();
+					return simulator.outputData.blockedByAgents[AgentIndex*SimulatorBurst.MaxBlockingAgentCount] != -1;
+				}
+			}
+
+			public void SetObstacleQuery (GraphNode sourceNode) {
+				HierarchicalNodeIndex = sourceNode != null && !sourceNode.Destroyed && sourceNode.Walkable ? sourceNode.HierarchicalNodeIndex : -1;
+			}
+
+			public void SetTarget (Vector3 targetPoint, float desiredSpeed, float maxSpeed, Vector3 endOfPath) {
+				simulator.simulationData.SetTarget(AgentIndex, targetPoint, desiredSpeed, maxSpeed, endOfPath);
+			}
+
+			public void SetCollisionNormal (Vector3 normal) {
+				simulator.simulationData.collisionNormal[AgentIndex] = normal;
+			}
+
+			public void ForceSetVelocity (Vector3 velocity) {
+				// A bit hacky, but it is approximately correct
+				// assuming the agent does not move significantly
+				simulator.simulationData.targetPoint[AgentIndex] = simulator.simulationData.position[AgentIndex] + (float3)velocity * 1000;
+				simulator.simulationData.desiredSpeed[AgentIndex] = velocity.magnitude;
+				simulator.simulationData.allowedVelocityDeviationAngles[AgentIndex] = float2.zero;
+				simulator.simulationData.manuallyControlled[AgentIndex] = true;
+			}
+		}
+
+		/// <summary>Holds internal obstacle data for the local avoidance simulation</summary>
+		public struct ObstacleData {
+			/// <summary>
+			/// Groups of vertices representing obstacles.
+			/// An obstacle is either a cycle or a chain of vertices
+			/// </summary>
+			public SlabAllocator<ObstacleVertexGroup> obstacleVertexGroups;
+			/// <summary>Vertices of all obstacles</summary>
+			public SlabAllocator<float3> obstacleVertices;
+			/// <summary>Obstacle sets, each one is represented as a set of obstacle vertex groups</summary>
+			public NativeList<UnmanagedObstacle> obstacles;
+
+			public void Init (Allocator allocator) {
+				if (!obstacles.IsCreated) obstacles = new NativeList<UnmanagedObstacle>(0, allocator);
+				if (!obstacleVertexGroups.IsCreated) obstacleVertexGroups = new SlabAllocator<ObstacleVertexGroup>(4, allocator);
+				if (!obstacleVertices.IsCreated) obstacleVertices = new SlabAllocator<float3>(16, allocator);
+			}
+
+			public void Dispose () {
+				if (obstacleVertexGroups.IsCreated) {
+					obstacleVertexGroups.Dispose();
+					obstacleVertices.Dispose();
+					obstacles.Dispose();
+				}
+			}
+		}
+
+		/// <summary>Holds internal agent data for the local avoidance simulation</summary>
+		public struct AgentData {
+			// Note: All 3D vectors are in world space
+			public NativeArray<AgentIndex> version;
+			public NativeArray<float> radius;
+			public NativeArray<float> height;
+			public NativeArray<float> desiredSpeed;
+			public NativeArray<float> maxSpeed;
+			public NativeArray<float> agentTimeHorizon;
+			public NativeArray<float> obstacleTimeHorizon;
+			public NativeArray<bool> locked;
+			public NativeArray<int> maxNeighbours;
+			public NativeArray<RVOLayer> layer;
+			public NativeArray<RVOLayer> collidesWith;
+			public NativeArray<float> flowFollowingStrength;
+			public NativeArray<float3> position;
+			public NativeArray<float3> collisionNormal;
+			public NativeArray<bool> manuallyControlled;
+			public NativeArray<float> priority;
+			public NativeArray<AgentDebugFlags> debugFlags;
+			public NativeArray<float3> targetPoint;
+			/// <summary>x = signed left angle in radians, y = signed right angle in radians (should be greater than x)</summary>
+			public NativeArray<float2> allowedVelocityDeviationAngles;
+			public NativeArray<NativeMovementPlane> movementPlane;
+			public NativeArray<float3> endOfPath;
+			/// <summary>Which obstacle data in the <see cref="ObstacleData.obstacles"/> array the agent should use for avoidance</summary>
+			public NativeArray<int> agentObstacleMapping;
+			public NativeArray<int> hierarchicalNodeIndex;
+
+			public void Realloc (int size, Allocator allocator) {
+				Util.Memory.Realloc(ref version, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref radius, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref height, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref desiredSpeed, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref maxSpeed, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref agentTimeHorizon, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref obstacleTimeHorizon, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref locked, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref maxNeighbours, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref layer, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref collidesWith, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref flowFollowingStrength, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref position, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref collisionNormal, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref manuallyControlled, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref priority, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref debugFlags, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref targetPoint, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref movementPlane, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref allowedVelocityDeviationAngles, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref endOfPath, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref agentObstacleMapping, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref hierarchicalNodeIndex, size, allocator, NativeArrayOptions.UninitializedMemory);
+			}
+
+			public void SetTarget (int agentIndex, float3 targetPoint, float desiredSpeed, float maxSpeed, float3 endOfPath) {
+				maxSpeed = math.max(maxSpeed, 0);
+				desiredSpeed = math.clamp(desiredSpeed, 0, maxSpeed);
+
+				this.targetPoint[agentIndex] = targetPoint;
+				this.desiredSpeed[agentIndex] = desiredSpeed;
+				this.maxSpeed[agentIndex] = maxSpeed;
+				this.endOfPath[agentIndex] = endOfPath;
+				// TODO: Set allowedVelocityDeviationAngles here
+			}
+
+			[System.Runtime.CompilerServices.MethodImpl(System.Runtime.CompilerServices.MethodImplOptions.AggressiveInlining)]
+			public bool HasDebugFlag(int agentIndex, AgentDebugFlags flag) => Unity.Burst.CompilerServices.Hint.Unlikely((debugFlags[agentIndex] & flag) != 0);
+
+			public void Dispose () {
+				version.Dispose();
+				radius.Dispose();
+				height.Dispose();
+				desiredSpeed.Dispose();
+				maxSpeed.Dispose();
+				agentTimeHorizon.Dispose();
+				obstacleTimeHorizon.Dispose();
+				locked.Dispose();
+				maxNeighbours.Dispose();
+				layer.Dispose();
+				collidesWith.Dispose();
+				flowFollowingStrength.Dispose();
+				position.Dispose();
+				collisionNormal.Dispose();
+				manuallyControlled.Dispose();
+				priority.Dispose();
+				debugFlags.Dispose();
+				targetPoint.Dispose();
+				movementPlane.Dispose();
+				allowedVelocityDeviationAngles.Dispose();
+				endOfPath.Dispose();
+				agentObstacleMapping.Dispose();
+				hierarchicalNodeIndex.Dispose();
+			}
+		};
+
+		public struct AgentOutputData {
+			public NativeArray<float3> targetPoint;
+			public NativeArray<float> speed;
+			public NativeArray<int> numNeighbours;
+			[NativeDisableParallelForRestrictionAttribute]
+			public NativeArray<int> blockedByAgents;
+			public NativeArray<ReachedEndOfPath> effectivelyReachedDestination;
+			public NativeArray<float> forwardClearance;
+
+			public void Realloc (int size, Allocator allocator) {
+				Util.Memory.Realloc(ref targetPoint, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref speed, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref numNeighbours, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref blockedByAgents, size * MaxBlockingAgentCount, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref effectivelyReachedDestination, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref forwardClearance, size, allocator, NativeArrayOptions.UninitializedMemory);
+			}
+
+			public void Move (int fromIndex, int toIndex) {
+				targetPoint[toIndex] = targetPoint[fromIndex];
+				speed[toIndex] = speed[fromIndex];
+				numNeighbours[toIndex] = numNeighbours[fromIndex];
+				effectivelyReachedDestination[toIndex] = effectivelyReachedDestination[fromIndex];
+				for (int i = 0; i < MaxBlockingAgentCount; i++) {
+					blockedByAgents[toIndex * MaxBlockingAgentCount + i] = blockedByAgents[fromIndex * MaxBlockingAgentCount + i];
+				}
+				forwardClearance[toIndex] = forwardClearance[fromIndex];
+			}
+
+			public void Dispose () {
+				targetPoint.Dispose();
+				speed.Dispose();
+				numNeighbours.Dispose();
+				blockedByAgents.Dispose();
+				effectivelyReachedDestination.Dispose();
+				forwardClearance.Dispose();
+			}
+		};
+
+		public struct HorizonAgentData {
+			public NativeArray<int> horizonSide;
+			public NativeArray<float> horizonMinAngle;
+			public NativeArray<float> horizonMaxAngle;
+
+			public void Realloc (int size, Allocator allocator) {
+				Util.Memory.Realloc(ref horizonSide, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref horizonMinAngle, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref horizonMaxAngle, size, allocator, NativeArrayOptions.UninitializedMemory);
+			}
+
+			public void Move (int fromIndex, int toIndex) {
+				horizonSide[toIndex] = horizonSide[fromIndex];
+				// The other values are temporary values that don't have to be moved
+			}
+
+			public void Dispose () {
+				horizonSide.Dispose();
+				horizonMinAngle.Dispose();
+				horizonMaxAngle.Dispose();
+			}
+		}
+
+		public struct TemporaryAgentData {
+			public NativeArray<float2> desiredTargetPointInVelocitySpace;
+			public NativeArray<float3> desiredVelocity;
+			public NativeArray<float3> currentVelocity;
+			public NativeArray<float2> collisionVelocityOffsets;
+			public NativeArray<int> neighbours;
+
+			public void Realloc (int size, Allocator allocator) {
+				Util.Memory.Realloc(ref desiredTargetPointInVelocitySpace, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref desiredVelocity, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref currentVelocity, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref collisionVelocityOffsets, size, allocator, NativeArrayOptions.UninitializedMemory);
+				Util.Memory.Realloc(ref neighbours, size * MaxNeighbourCount, allocator, NativeArrayOptions.UninitializedMemory);
+			}
+
+			public void Dispose () {
+				desiredTargetPointInVelocitySpace.Dispose();
+				desiredVelocity.Dispose();
+				currentVelocity.Dispose();
+				neighbours.Dispose();
+				collisionVelocityOffsets.Dispose();
+			}
+		}
+
+		/// <summary>
+		/// Time in seconds between each simulation step.
+		/// This is the desired delta time, the simulation will never run at a higher fps than
+		/// the rate at which the Update function is called.
+		/// </summary>
+		public float DesiredDeltaTime { get { return desiredDeltaTime; } set { desiredDeltaTime = System.Math.Max(value, 0.0f); } }
+
+		/// <summary>
+		/// Bias agents to pass each other on the right side.
+		/// If the desired velocity of an agent puts it on a collision course with another agent or an obstacle
+		/// its desired velocity will be rotated this number of radians (1 radian is approximately 57°) to the right.
+		/// This helps to break up symmetries and makes it possible to resolve some situations much faster.
+		///
+		/// When many agents have the same goal this can however have the side effect that the group
+		/// clustered around the target point may as a whole start to spin around the target point.
+		///
+		/// Recommended values are in the range of 0 to 0.2.
+		///
+		/// If this value is negative, the agents will be biased towards passing each other on the left side instead.
+		/// </summary>
+		public float SymmetryBreakingBias { get; set; }
+
+		/// <summary>Use hard collisions</summary>
+		public bool HardCollisions { get; set; }
+
+		public bool UseNavmeshAsObstacle { get; set; }
+
+		public Rect AgentBounds {
+			get {
+				lastJob.Complete();
+				return quadtree.bounds;
+			}
+		}
+
+		/// <summary>Number of agents in the simulation</summary>
+		public int AgentCount => numAgents;
+
+		public MovementPlane MovementPlane => movementPlane;
+
+		/// <summary>Determines if the XY (2D) or XZ (3D) plane is used for movement</summary>
+		public readonly MovementPlane movementPlane = MovementPlane.XZ;
+
+		/// <summary>Job handle for the last update</summary>
+		public JobHandle lastJob { get; private set; }
+
+		public void BlockUntilSimulationStepDone () {
+			lastJob.Complete();
+		}
+
+		/// <summary>Create a new simulator.</summary>
+		/// <param name="movementPlane">The plane that the movement happens in. XZ for 3D games, XY for 2D games.</param>
+		public SimulatorBurst (MovementPlane movementPlane) {
+			this.DesiredDeltaTime = 1;
+			this.movementPlane = movementPlane;
+
+			obstacleData.Init(Allocator.Persistent);
+			AllocateAgentSpace();
+
+			// Just to make sure the quadtree is in a valid state
+			quadtree.BuildJob(simulationData.position, simulationData.version, simulationData.desiredSpeed, simulationData.radius, 0, movementPlane).Run();
+		}
+
+		/// <summary>Removes all agents from the simulation</summary>
+		public void ClearAgents () {
+			BlockUntilSimulationStepDone();
+			for (int i = 0; i < agentDestroyCallbacks.Length; i++) agentDestroyCallbacks[i]?.Invoke();
+			numAgents = 0;
+		}
+
+		/// <summary>
+		/// Frees all used memory.
+		/// Warning: You must call this when you are done with the simulator, otherwise some resources can linger and lead to memory leaks.
+		/// </summary>
+		public void OnDestroy () {
+			debugDrawingScope.Dispose();
+			BlockUntilSimulationStepDone();
+			ClearAgents();
+			obstacleData.Dispose();
+			simulationData.Dispose();
+			temporaryAgentData.Dispose();
+			outputData.Dispose();
+			quadtree.Dispose();
+			horizonAgentData.Dispose();
+		}
+
+		void AllocateAgentSpace () {
+			if (numAgents > agentPreCalculationCallbacks.Length || agentPreCalculationCallbacks.Length == 0) {
+				var prevSize = simulationData.version.Length;
+				int newSize = Mathf.Max(64, Mathf.Max(numAgents, agentPreCalculationCallbacks.Length * 2));
+				simulationData.Realloc(newSize, Allocator.Persistent);
+				temporaryAgentData.Realloc(newSize, Allocator.Persistent);
+				outputData.Realloc(newSize, Allocator.Persistent);
+				horizonAgentData.Realloc(newSize, Allocator.Persistent);
+				Memory.Realloc(ref agentPreCalculationCallbacks, newSize);
+				Memory.Realloc(ref agentDestroyCallbacks, newSize);
+				for (int i = prevSize; i < newSize; i++) simulationData.version[i] = new AgentIndex(0, i);
+			}
+		}
+
+		/// <summary>
+		/// Add an agent at the specified position.
+		/// You can use the returned interface to read and write parameters
+		/// and set for example radius and desired point to move to.
+		///
+		/// See: <see cref="RemoveAgent"/>
+		///
+		/// Deprecated: Use AddAgent(Vector3) instead
+		/// </summary>
+		[System.Obsolete("Use AddAgent(Vector3) instead")]
+		public IAgent AddAgent (Vector2 position, float elevationCoordinate) {
+			if (movementPlane == MovementPlane.XY) return AddAgent(new Vector3(position.x, position.y, elevationCoordinate));
+			else return AddAgent(new Vector3(position.x, elevationCoordinate, position.y));
+		}
+
+		/// <summary>
+		/// Add an agent at the specified position.
+		/// You can use the returned interface to read and write parameters
+		/// and set for example radius and desired point to move to.
+		///
+		/// See: <see cref="RemoveAgent"/>
+		/// </summary>
+		/// <param name="position">See \reflink{IAgent.Position}</param>
+		public IAgent AddAgent (Vector3 position) {
+			var agentIndex = AddAgentBurst(position);
+			return new Agent { simulator = this, agentIndex = agentIndex };
+		}
+
+		/// <summary>
+		/// Add an agent at the specified position.
+		/// You can use the returned index to read and write parameters
+		/// and set for example radius and desired point to move to.
+		///
+		/// See: <see cref="RemoveAgent"/>
+		/// </summary>
+		public AgentIndex AddAgentBurst (float3 position) {
+			BlockUntilSimulationStepDone();
+
+			int agentIndex;
+			if (freeAgentIndices.Count > 0) {
+				agentIndex = freeAgentIndices.Pop();
+			} else {
+				agentIndex = numAgents++;
+				AllocateAgentSpace();
+			}
+
+			var packedAgentIndex = simulationData.version[agentIndex].WithIncrementedVersion();
+			UnityEngine.Assertions.Assert.AreEqual(packedAgentIndex.Index, agentIndex);
+
+			simulationData.version[agentIndex] = packedAgentIndex;
+			simulationData.radius[agentIndex] = 5;
+			simulationData.height[agentIndex] = 5;
+			simulationData.desiredSpeed[agentIndex] = 0;
+			simulationData.maxSpeed[agentIndex] = 1;
+			simulationData.agentTimeHorizon[agentIndex] = 2;
+			simulationData.obstacleTimeHorizon[agentIndex] = 2;
+			simulationData.locked[agentIndex] = false;
+			simulationData.maxNeighbours[agentIndex] = 10;
+			simulationData.layer[agentIndex] = RVOLayer.DefaultAgent;
+			simulationData.collidesWith[agentIndex] = (RVOLayer)(-1);
+			simulationData.flowFollowingStrength[agentIndex] = 0;
+			simulationData.position[agentIndex] = position;
+			simulationData.collisionNormal[agentIndex] = float3.zero;
+			simulationData.manuallyControlled[agentIndex] = false;
+			simulationData.priority[agentIndex] = 0.5f;
+			simulationData.debugFlags[agentIndex] = AgentDebugFlags.Nothing;
+			simulationData.targetPoint[agentIndex] = position;
+			// Set the default movement plane. Default to the XZ plane even if movement plane is arbitrary (the user will have to set a custom one later)
+			simulationData.movementPlane[agentIndex] = new NativeMovementPlane((movementPlane == MovementPlane.XY ? SimpleMovementPlane.XYPlane : SimpleMovementPlane.XZPlane).rotation);
+			simulationData.allowedVelocityDeviationAngles[agentIndex] = float2.zero;
+			simulationData.endOfPath[agentIndex] = float3.zero;
+			simulationData.agentObstacleMapping[agentIndex] = -1;
+			simulationData.hierarchicalNodeIndex[agentIndex] = -1;
+
+			outputData.speed[agentIndex] = 0;
+			outputData.numNeighbours[agentIndex] = 0;
+			outputData.targetPoint[agentIndex] = position;
+			outputData.blockedByAgents[agentIndex * MaxBlockingAgentCount] = -1;
+			outputData.effectivelyReachedDestination[agentIndex] = ReachedEndOfPath.NotReached;
+
+			horizonAgentData.horizonSide[agentIndex] = 0;
+			agentPreCalculationCallbacks[agentIndex] = null;
+			agentDestroyCallbacks[agentIndex] = null;
+
+			return packedAgentIndex;
+		}
+
+		/// <summary>Deprecated: Use AddAgent(Vector3) instead</summary>
+		[System.Obsolete("Use AddAgent(Vector3) instead")]
+		public IAgent AddAgent (IAgent agent) {
+			throw new System.NotImplementedException("Use AddAgent(position) instead. Agents are not persistent after being removed.");
+		}
+
+		/// <summary>
+		/// Removes a specified agent from this simulation.
+		/// The agent can be added again later by using AddAgent.
+		///
+		/// See: AddAgent(IAgent)
+		/// See: ClearAgents
+		/// </summary>
+		public void RemoveAgent (IAgent agent) {
+			if (agent == null) throw new System.ArgumentNullException(nameof(agent));
+			Agent realAgent = (Agent)agent;
+			RemoveAgent(realAgent.agentIndex);
+		}
+
+		public bool AgentExists (AgentIndex agent) {
+			BlockUntilSimulationStepDone();
+			if (!simulationData.version.IsCreated) return false;
+			var index = agent.Index;
+			if (index >= simulationData.version.Length) return false;
+			if (agent.Version != simulationData.version[index].Version) return false;
+			return true;
+		}
+
+		public void RemoveAgent (AgentIndex agent) {
+			BlockUntilSimulationStepDone();
+			if (!AgentExists(agent)) throw new System.InvalidOperationException("Trying to remove agent which does not exist");
+
+			var index = agent.Index;
+			// Increment version and set deleted bit
+			simulationData.version[index] = simulationData.version[index].WithIncrementedVersion().WithDeleted();
+			// Avoid memory leaks
+			agentPreCalculationCallbacks[index] = null;
+			try {
+				if (agentDestroyCallbacks[index] != null) agentDestroyCallbacks[index]();
+			} catch (System.Exception e) {
+				Debug.LogException(e);
+			}
+			agentDestroyCallbacks[index] = null;
+			freeAgentIndices.Push(index);
+		}
+
+		void PreCalculation (JobHandle dependency) {
+			bool blocked = false;
+			for (int i = 0; i < numAgents; i++) {
+				var callback = agentPreCalculationCallbacks[i];
+				if (callback != null) {
+					if (!blocked) {
+						dependency.Complete();
+						blocked = true;
+					}
+					callback.Invoke();
+				}
+			}
+		}
+
+		/// <summary>Should be called once per frame.</summary>
+		/// <param name="dependency">Jobs that need to complete before local avoidance runs.</param>
+		/// <param name="dt">Length of timestep in seconds.</param>
+		/// <param name="drawGizmos">If true, debug gizmos will be allowed to render (they never render in standalone games, though).</param>
+		/// <param name="allocator">Allocator to use for some temporary allocations. Should be a rewindable allocator since no disposal will be done.</param>
+		public JobHandle Update (JobHandle dependency, float dt, bool drawGizmos, Allocator allocator) {
+			var x = 0;
+			if (x != 0) {
+				// We need to specify these types somewhere in their concrete form.
+				// Otherwise the burst compiler doesn't understand that it has to compile them.
+				// This code will never run.
+				new JobRVO<XYMovementPlane>().ScheduleBatch(0, 0);
+				new JobRVO<XZMovementPlane>().ScheduleBatch(0, 0);
+				new JobRVO<ArbitraryMovementPlane>().ScheduleBatch(0, 0);
+
+				new JobRVOCalculateNeighbours<XYMovementPlane>().ScheduleBatch(0, 0);
+				new JobRVOCalculateNeighbours<XZMovementPlane>().ScheduleBatch(0, 0);
+				new JobRVOCalculateNeighbours<ArbitraryMovementPlane>().ScheduleBatch(0, 0);
+
+				new JobHardCollisions<XYMovementPlane>().ScheduleBatch(0, 0);
+				new JobHardCollisions<XZMovementPlane>().ScheduleBatch(0, 0);
+				new JobHardCollisions<ArbitraryMovementPlane>().ScheduleBatch(0, 0);
+
+				new JobDestinationReached<XYMovementPlane>().Schedule();
+				new JobDestinationReached<XZMovementPlane>().Schedule();
+				new JobDestinationReached<ArbitraryMovementPlane>().Schedule();
+			}
+
+			// The burst jobs are specialized for the type of movement plane used. This improves performance for the XY and XZ movement planes quite a lot
+			if (movementPlane == MovementPlane.XY) return UpdateInternal<XYMovementPlane>(dependency, dt, drawGizmos, allocator);
+			else if (movementPlane == MovementPlane.XZ) return UpdateInternal<XZMovementPlane>(dependency, dt, drawGizmos, allocator);
+			else return UpdateInternal<ArbitraryMovementPlane>(dependency, dt, drawGizmos, allocator);
+		}
+
+		public void LockSimulationDataReadOnly (JobHandle dependencies) {
+			this.lastJob = JobHandle.CombineDependencies(this.lastJob, dependencies);
+		}
+
+		JobHandle UpdateInternal<T>(JobHandle dependency, float deltaTime, bool drawGizmos, Allocator allocator) where T : struct, IMovementPlaneWrapper {
+			// Prevent a zero delta time
+			deltaTime = math.max(deltaTime, 1.0f/2000f);
+
+			BlockUntilSimulationStepDone();
+
+			UnityEngine.Profiling.Profiler.BeginSample("Read agent data");
+
+			// Read agent data from RVOController components on the main thread.
+			// We cannot do this in a job because RVOController data may be changed at any time
+			// on the main thread.
+			PreCalculation(dependency);
+
+			UnityEngine.Profiling.Profiler.EndSample();
+
+			var quadtreeJob = quadtree.BuildJob(simulationData.position, simulationData.version, outputData.speed, simulationData.radius, numAgents, movementPlane).Schedule(dependency);
+
+			var preprocessJob = new JobRVOPreprocess {
+				agentData = simulationData,
+				previousOutput = outputData,
+				temporaryAgentData = temporaryAgentData,
+				startIndex = 0,
+				endIndex = numAgents,
+			}.Schedule(dependency);
+
+			int batchSize = math.max(numAgents / 64, 8);
+			var neighboursJob = new JobRVOCalculateNeighbours<T> {
+				agentData = simulationData,
+				quadtree = quadtree,
+				outNeighbours = temporaryAgentData.neighbours,
+				output = outputData,
+			}.ScheduleBatch(numAgents, batchSize, JobHandle.CombineDependencies(preprocessJob, quadtreeJob));
+
+			// Make the threads start working now, we have enough work scheduled that they have stuff to do.
+			JobHandle.ScheduleBatchedJobs();
+
+			var combinedJob = JobHandle.CombineDependencies(preprocessJob, neighboursJob);
+
+			debugDrawingScope.Rewind();
+			var draw = DrawingManager.GetBuilder(debugDrawingScope);
+
+			var horizonJob1 = new JobHorizonAvoidancePhase1 {
+				agentData = simulationData,
+				neighbours = temporaryAgentData.neighbours,
+				desiredTargetPointInVelocitySpace = temporaryAgentData.desiredTargetPointInVelocitySpace,
+				horizonAgentData = horizonAgentData,
+				draw = draw,
+			}.ScheduleBatch(numAgents, batchSize, combinedJob);
+
+			var horizonJob2 = new JobHorizonAvoidancePhase2 {
+				neighbours = temporaryAgentData.neighbours,
+				versions = simulationData.version,
+				desiredVelocity = temporaryAgentData.desiredVelocity,
+				desiredTargetPointInVelocitySpace = temporaryAgentData.desiredTargetPointInVelocitySpace,
+				horizonAgentData = horizonAgentData,
+				movementPlane = simulationData.movementPlane,
+			}.ScheduleBatch(numAgents, batchSize, horizonJob1);
+
+			var hardCollisionsJob1 = new JobHardCollisions<T> {
+				agentData = simulationData,
+				neighbours = temporaryAgentData.neighbours,
+				collisionVelocityOffsets = temporaryAgentData.collisionVelocityOffsets,
+				deltaTime = deltaTime,
+				enabled = HardCollisions,
+			}.ScheduleBatch(numAgents, batchSize, combinedJob);
+
+			RWLock.CombinedReadLockAsync navmeshEdgeDataLock;
+			NavmeshEdges.NavmeshBorderData navmeshEdgeData;
+			bool hasAstar = AstarPath.active != null;
+			if (hasAstar) {
+				navmeshEdgeData = AstarPath.active.GetNavmeshBorderData(out navmeshEdgeDataLock);
+			} else {
+				navmeshEdgeData = NavmeshEdges.NavmeshBorderData.CreateEmpty(allocator);
+				navmeshEdgeDataLock = default;
+			}
+			var rvoJobData = new JobRVO<T> {
+				agentData = simulationData,
+				temporaryAgentData = temporaryAgentData,
+				navmeshEdgeData = navmeshEdgeData,
+				output = outputData,
+				deltaTime = deltaTime,
+				symmetryBreakingBias = Mathf.Max(0, SymmetryBreakingBias),
+				draw = draw,
+				useNavmeshAsObstacle = UseNavmeshAsObstacle,
+				priorityMultiplier = 1f,
+				// priorityMultiplier = 0.1f,
+			};
+
+			combinedJob = JobHandle.CombineDependencies(horizonJob2, hardCollisionsJob1, navmeshEdgeDataLock.dependency);
+
+			// JobHandle rvoJob = combinedJob;
+			// for (int k = 0; k < 3; k++) {
+			// 	var preprocessJob2 = new JobRVOPreprocess {
+			// 		agentData = simulationData,
+			// 		previousOutput = outputData,
+			// 		temporaryAgentData = temporaryAgentData,
+			// 		startIndex = 0,
+			// 		endIndex = numAgents,
+			// 	}.Schedule(rvoJob);
+			// 	rvoJob = new JobRVO<T> {
+			// 		agentData = simulationData,
+			// 		temporaryAgentData = temporaryAgentData,
+			// 		navmeshEdgeData = navmeshEdgeData,
+			// 		output = outputData,
+			// 		deltaTime = deltaTime,
+			// 		symmetryBreakingBias = Mathf.Max(0, SymmetryBreakingBias),
+			// 		draw = draw,
+			// 		priorityMultiplier = (k+1) * (1.0f/3.0f),
+			// 	}.ScheduleBatch(numAgents, batchSize, preprocessJob2);
+			// }
+			var rvoJob = rvoJobData.ScheduleBatch(numAgents, batchSize, combinedJob);
+			if (hasAstar) {
+				navmeshEdgeDataLock.UnlockAfter(rvoJob);
+			} else {
+				navmeshEdgeData.DisposeEmpty(rvoJob);
+			}
+
+			var reachedJob = new JobDestinationReached<T> {
+				agentData = simulationData,
+				obstacleData = obstacleData,
+				temporaryAgentData = temporaryAgentData,
+				output = outputData,
+				draw = draw,
+				numAgents = numAgents,
+			}.Schedule(rvoJob);
+
+			// Clear some fields that are reset every simulation tick
+			var clearJob = simulationData.collisionNormal.MemSet(float3.zero).Schedule(reachedJob);
+			var clearJob2 = simulationData.manuallyControlled.MemSet(false).Schedule(reachedJob);
+			var clearJob3 = simulationData.hierarchicalNodeIndex.MemSet(-1).Schedule(reachedJob);
+
+			dependency = JobHandle.CombineDependencies(reachedJob, clearJob, clearJob2);
+			dependency = JobHandle.CombineDependencies(dependency, clearJob3);
+
+			if (drawQuadtree && drawGizmos) {
+				dependency = JobHandle.CombineDependencies(dependency, new RVOQuadtreeBurst.DebugDrawJob {
+					draw = draw,
+					quadtree = quadtree,
+				}.Schedule(quadtreeJob));
+			}
+
+			draw.DisposeAfter(dependency);
+
+			lastJob = dependency;
+			return dependency;
+		}
+	}
+}
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulatorBurst.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulatorBurst.cs.meta
new file mode 100644
index 0000000..55b77cd
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOCoreSimulatorBurst.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 0b618542be33248ba974eb1f4c16e73b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOObstacleCache.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOObstacleCache.cs
new file mode 100644
index 0000000..5261871
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOObstacleCache.cs
@@ -0,0 +1,344 @@
+namespace Pathfinding.RVO {
+	using Pathfinding;
+	using UnityEngine;
+	using Pathfinding.Util;
+	using Unity.Mathematics;
+	using Unity.Collections;
+	using Unity.Jobs;
+	using System.Collections.Generic;
+	using Unity.Burst;
+	using Unity.Profiling;
+	using Pathfinding.Jobs;
+#if MODULE_COLLECTIONS_2_1_0_OR_NEWER
+	using NativeHashMapIntInt = Unity.Collections.NativeHashMap<int, int>;
+#else
+	using NativeHashMapIntInt = Unity.Collections.NativeParallelHashMap<int, int>;
+#endif
+
+	[BurstCompile]
+	public static class RVOObstacleCache {
+		public struct ObstacleSegment {
+			public float3 vertex1;
+			public float3 vertex2;
+			public int vertex1LinkId;
+			public int vertex2LinkId;
+		}
+
+		static ulong HashKey (GraphNode sourceNode, int traversableTags, SimpleMovementPlane movementPlane) {
+			var hash = (ulong)sourceNode.NodeIndex;
+			hash = hash * 786433 ^ (ulong)traversableTags;
+			// The rotation is not particularly important for the obstacle. It's only used
+			// to simplify the output a bit. So we allow similar rotations to share the same hash.
+			const float RotationQuantization = 4;
+			hash = hash * 786433 ^ (ulong)(movementPlane.rotation.x*RotationQuantization);
+			hash = hash * 786433 ^ (ulong)(movementPlane.rotation.y*RotationQuantization);
+			hash = hash * 786433 ^ (ulong)(movementPlane.rotation.z*RotationQuantization);
+			hash = hash * 786433 ^ (ulong)(movementPlane.rotation.w*RotationQuantization);
+			return hash;
+		}
+
+		/// <summary>
+		/// Collects an unordered list of contour segments based on the given nodes.
+		///
+		/// Note: All nodes must be from the same graph.
+		/// </summary>
+		public static void CollectContours (List<GraphNode> nodes, NativeList<ObstacleSegment> obstacles) {
+			if (nodes.Count == 0) return;
+			if (nodes[0] is TriangleMeshNode) {
+				for (int i = 0; i < nodes.Count; i++) {
+					var tnode = nodes[i] as TriangleMeshNode;
+					var used = 0;
+					if (tnode.connections != null) {
+						for (int j = 0; j < tnode.connections.Length; j++) {
+							var conn = tnode.connections[j];
+							if (conn.isEdgeShared) {
+								used |= 1 << conn.shapeEdge;
+							}
+						}
+					}
+
+					tnode.GetVertices(out var v0, out var v1, out var v2);
+					for (int edgeIndex = 0; edgeIndex < 3; edgeIndex++) {
+						if ((used & (1 << edgeIndex)) == 0) {
+							// This edge is not shared, therefore it's a contour edge
+							Int3 leftVertex, rightVertex;
+							switch (edgeIndex) {
+							case 0:
+								leftVertex = v0;
+								rightVertex = v1;
+								break;
+							case 1:
+								leftVertex = v1;
+								rightVertex = v2;
+								break;
+							case 2:
+							default:
+								leftVertex = v2;
+								rightVertex = v0;
+								break;
+							}
+							var leftVertexHash = leftVertex.GetHashCode();
+							var rightVertexHash = rightVertex.GetHashCode();
+
+							obstacles.Add(new ObstacleSegment {
+								vertex1 = (Vector3)leftVertex,
+								vertex2 = (Vector3)rightVertex,
+								vertex1LinkId = leftVertexHash,
+								vertex2LinkId = rightVertexHash,
+							});
+						}
+					}
+				}
+			} else if (nodes[0] is GridNodeBase) {
+				GridGraph graph;
+				if (nodes[0] is LevelGridNode) graph = LevelGridNode.GetGridGraph(nodes[0].GraphIndex);
+				else
+					graph = GridNode.GetGridGraph(nodes[0].GraphIndex);
+				unsafe {
+					// Offsets from the center of the node to the corners of the node, in world space
+					// Index dir is the offset to the left corner of the edge in direction dir
+					// See GridNodeBase.GetNeighbourAlongDirection for the direction indices
+					Vector3* offsets = stackalloc Vector3[4];
+					for (int dir = 0; dir < 4; dir++) {
+						var dl = (dir + 1) % 4;
+						offsets[dir] = graph.transform.TransformVector(0.5f * new Vector3(GridGraph.neighbourXOffsets[dir] + GridGraph.neighbourXOffsets[dl], 0, GridGraph.neighbourZOffsets[dir] + GridGraph.neighbourZOffsets[dl]));
+					}
+
+					for (int i = 0; i < nodes.Count; i++) {
+						var gnode = nodes[i] as GridNodeBase;
+						if (gnode.HasConnectionsToAllAxisAlignedNeighbours) continue;
+
+						for (int dir = 0; dir < 4; dir++) {
+							if (!gnode.HasConnectionInDirection(dir)) {
+								// ┌─────────┬─────────┐
+								// │         │         │
+								// │   nl1   │   nl2   │     ^
+								// │         │         │     |
+								// ├────────vL─────────┤     dl
+								// │         │#########│
+								// │   node  │#########│     dir->
+								// │         │#########│
+								// ├────────vR─────────┤     dr
+								// │         │         │      |
+								// │   nr1   │   nr2   │      v
+								// │         │         │
+								// └─────────┴─────────┘
+								var dl = (dir + 1) % 4;
+								var dr = (dir - 1 + 4) % 4;
+								var nl1 = gnode.GetNeighbourAlongDirection(dl);
+								var nr1 = gnode.GetNeighbourAlongDirection(dr);
+
+								// All this hashing code looks slow, but really it's not compared to all the memory accesses to get the node data
+								uint leftVertexHash;
+								if (nl1 != null) {
+									var nl2 = nl1.GetNeighbourAlongDirection(dir);
+									if (nl2 != null) {
+										// Outer corner. Uniquely determined by the 3 nodes that touch the corner.
+										var a = gnode.NodeIndex;
+										var b = nl1.NodeIndex;
+										var c = nl2.NodeIndex;
+										// Sort the values so that a <= b <= c
+										if (a > b) Memory.Swap(ref a, ref b);
+										if (b > c) Memory.Swap(ref b, ref c);
+										if (a > b) Memory.Swap(ref a, ref b);
+										leftVertexHash = math.hash(new uint3(a, b, c));
+									} else {
+										// Straight wall. Uniquely determined by the 2 nodes that touch the corner and the direction to the wall.
+										var a = gnode.NodeIndex;
+										var b = nl1.NodeIndex;
+										// Sort the values so that a <= b
+										if (a > b) Memory.Swap(ref a, ref b);
+										leftVertexHash = math.hash(new uint3(a, b, (uint)dir));
+									}
+								} else {
+									// Inner corner. Uniquely determined by the single node that touches the corner and the direction to it.
+									var diagonalToCorner = dir + 4;
+									leftVertexHash = math.hash(new uint2(gnode.NodeIndex, (uint)diagonalToCorner));
+								}
+
+								uint rightVertexHash;
+								if (nr1 != null) {
+									var nr2 = nr1.GetNeighbourAlongDirection(dir);
+									if (nr2 != null) {
+										// Outer corner. Uniquely determined by the 3 nodes that touch the corner.
+										var a = gnode.NodeIndex;
+										var b = nr1.NodeIndex;
+										var c = nr2.NodeIndex;
+										// Sort the values so that a <= b <= c
+										if (a > b) Memory.Swap(ref a, ref b);
+										if (b > c) Memory.Swap(ref b, ref c);
+										if (a > b) Memory.Swap(ref a, ref b);
+										rightVertexHash = math.hash(new uint3(a, b, c));
+									} else {
+										// Straight wall. Uniquely determined by the 2 nodes that touch the corner and the direction to the wall.
+										var a = gnode.NodeIndex;
+										var b = nr1.NodeIndex;
+										// Sort the values so that a <= b
+										if (a > b) Memory.Swap(ref a, ref b);
+										rightVertexHash = math.hash(new uint3(a, b, (uint)dir));
+									}
+								} else {
+									// Inner corner. Uniquely determined by the single node that touches the corner and the direction to it.
+									// Note: It's not a typo that we use `dr+4` here and `dir+4` above. They are different directions.
+									var diagonalToCorner = dr + 4;
+									rightVertexHash = math.hash(new uint2(gnode.NodeIndex, (uint)diagonalToCorner));
+								}
+
+								var nodePos = (Vector3)gnode.position;
+								obstacles.Add(new ObstacleSegment {
+									vertex1 = nodePos + offsets[dir], // Left corner. Yes, it should be dir, not dl, as the offsets array already points to the left corners of each segment.
+									vertex2 = nodePos + offsets[dr], // Right corner
+									vertex1LinkId = (int)leftVertexHash,
+									vertex2LinkId = (int)rightVertexHash,
+								});
+							}
+						}
+					}
+				}
+			}
+		}
+
+		private static readonly ProfilerMarker MarkerAllocate = new ProfilerMarker("Allocate");
+
+		/// <summary>Trace contours generated by CollectContours.</summary>
+		/// <param name="obstaclesSpan">Obstacle segments, typically the borders of the navmesh. In no particular order.
+		///                  Each edge must be oriented the same way (e.g. all clockwise, or all counter-clockwise around the obstacles).</param>
+		/// <param name="movementPlane">The movement plane used for simplification. The up direction will be treated as less important for purposes of simplification.</param>
+		/// <param name="obstacleId">The ID of the obstacle to write into the outputObstacles array.</param>
+		/// <param name="outputObstacles">Array to write the obstacle to.</param>
+		/// <param name="verticesAllocator">Allocator to use for the vertices of the obstacle.</param>
+		/// <param name="obstaclesAllocator">Allocator to use for the obstacle metadata.</param>
+		/// <param name="spinLock">Lock to use when allocating from the allocators.</param>
+		/// <param name="simplifyObstacles">If true, the obstacle will be simplified. This means that colinear vertices (when projected onto the movement plane) will be removed.</param>
+		[BurstCompile]
+		public static unsafe void TraceContours (ref UnsafeSpan<ObstacleSegment> obstaclesSpan, ref NativeMovementPlane movementPlane, int obstacleId, UnmanagedObstacle* outputObstacles, ref SlabAllocator<float3> verticesAllocator, ref SlabAllocator<ObstacleVertexGroup> obstaclesAllocator, ref SpinLock spinLock, bool simplifyObstacles) {
+			var obstacles = obstaclesSpan;
+			if (obstacles.Length == 0) {
+				outputObstacles[obstacleId] = new UnmanagedObstacle {
+					verticesAllocation = SlabAllocator<float3>.ZeroLengthArray,
+					groupsAllocation = SlabAllocator<ObstacleVertexGroup>.ZeroLengthArray,
+				};
+				return;
+			}
+
+			MarkerAllocate.Begin();
+			var traceLookup = new NativeHashMapIntInt(obstacles.Length, Unity.Collections.Allocator.Temp);
+			// For each edge: Will be 0 if the segment should be ignored or if it has been visited, 1 if it has not been visited and has an ingoing edge, and 2 if it has not been visited and has no ingoing edge.
+			var priority = new NativeArray<byte>(obstacles.Length, Unity.Collections.Allocator.Temp, Unity.Collections.NativeArrayOptions.UninitializedMemory);
+			MarkerAllocate.End();
+			for (int i = 0; i < obstacles.Length; i++) {
+				// var obstacle = obstacles[i];
+				// Add the edge to the lookup. But if it already exists, ignore it.
+				// That it already exists is very much a special case that can happen if there is
+				// overlapping geometry (typically added by a NavmeshAdd component).
+				// In that case the "outer edge" that we want to trace is kinda undefined, but we will
+				// do our best with it.
+				if (traceLookup.TryAdd(obstacles[i].vertex1LinkId, i)) {
+					priority[i] = 2;
+				} else {
+					priority[i] = 0;
+				}
+			}
+			for (int i = 0; i < obstacles.Length; i++) {
+				if (traceLookup.TryGetValue(obstacles[i].vertex2LinkId, out var other) && priority[other] > 0) {
+					// The other segment has an ingoing edge. This means it cannot be the start of a contour.
+					// Reduce the priority so that we only consider it when looking for cycles.
+					priority[other] = 1;
+				}
+			}
+
+			var outputMetadata = new NativeList<ObstacleVertexGroup>(16, Allocator.Temp);
+			var outputVertices = new NativeList<float3>(16, Allocator.Temp);
+			// We now have a hashmap of directed edges (vertex1 -> vertex2) such that these edges are directed the same (cw or ccw), and "outer edges".
+			// We can now follow these directed edges to trace out the contours of the mesh.
+			var toPlaneMatrix = movementPlane.AsWorldToPlaneMatrix();
+			for (int allowLoops = 0; allowLoops <= 1; allowLoops++) {
+				var minPriority = allowLoops == 1 ? 1 : 2;
+				for (int i = 0; i < obstacles.Length; i++) {
+					if (priority[i] >= minPriority) {
+						int startVertices = outputVertices.Length;
+						outputVertices.Add(obstacles[i].vertex1);
+
+						var lastAdded = obstacles[i].vertex1;
+						var candidateVertex = obstacles[i].vertex2;
+						var index = i;
+						var obstacleType = ObstacleType.Chain;
+						var boundsMn = lastAdded;
+						var boundsMx = lastAdded;
+						while (true) {
+							if (priority[index] == 0) {
+								// This should not happen for a regular navmesh.
+								// But it can happen if there are degenerate edges or overlapping triangles.
+								// In that case we will just stop here
+								break;
+							}
+							priority[index] = 0;
+
+							float3 nextVertex;
+							if (traceLookup.TryGetValue(obstacles[index].vertex2LinkId, out int nextIndex)) {
+								nextVertex = 0.5f * (obstacles[index].vertex2 + obstacles[nextIndex].vertex1);
+							} else {
+								nextVertex = obstacles[index].vertex2;
+								nextIndex = -1;
+							}
+
+							// Try to simplify and see if we even need to add the vertex C.
+							var p1 = lastAdded;
+							var p2 = nextVertex;
+							var p3 = candidateVertex;
+							var d1 = toPlaneMatrix.ToPlane(p2 - p1);
+							var d2 = toPlaneMatrix.ToPlane(p3 - p1);
+							var det = VectorMath.Determinant(d1, d2);
+							if (math.abs(det) < 0.01f && simplifyObstacles) {
+								// We don't need to add candidateVertex. It's just a straight line (p1,p2,p3 are colinear).
+							} else {
+								outputVertices.Add(candidateVertex);
+								boundsMn = math.min(boundsMn, candidateVertex);
+								boundsMx = math.max(boundsMx, candidateVertex);
+								lastAdded = p3;
+							}
+
+							if (nextIndex == i) {
+								// Loop
+								outputVertices[startVertices] = nextVertex;
+								obstacleType = ObstacleType.Loop;
+								break;
+							} else if (nextIndex == -1) {
+								// End of chain
+								outputVertices.Add(nextVertex);
+								boundsMn = math.min(boundsMn, nextVertex);
+								boundsMx = math.max(boundsMx, nextVertex);
+								break;
+							}
+
+							index = nextIndex;
+							candidateVertex = nextVertex;
+						}
+
+						outputMetadata.Add(new ObstacleVertexGroup {
+							type = obstacleType,
+							vertexCount = outputVertices.Length - startVertices,
+							boundsMn = boundsMn,
+							boundsMx = boundsMx,
+						});
+					}
+				}
+			}
+
+			int obstacleSet, vertexSet;
+			if (outputMetadata.Length > 0) {
+				spinLock.Lock();
+				obstacleSet = obstaclesAllocator.Allocate(outputMetadata);
+				vertexSet = verticesAllocator.Allocate(outputVertices);
+				spinLock.Unlock();
+			} else {
+				obstacleSet = SlabAllocator<ObstacleVertexGroup>.ZeroLengthArray;
+				vertexSet = SlabAllocator<float3>.ZeroLengthArray;
+			}
+			outputObstacles[obstacleId] = new UnmanagedObstacle {
+				verticesAllocation = vertexSet,
+				groupsAllocation = obstacleSet,
+			};
+		}
+	}
+}
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOObstacleCache.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOObstacleCache.cs.meta
new file mode 100644
index 0000000..344ea02
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOObstacleCache.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 8a3a9a0945d2b43f5b8cb871ffbc1b2b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtree.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtree.cs
new file mode 100644
index 0000000..3b6ed49
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtree.cs
@@ -0,0 +1,4 @@
+
+// This file has been removed from the project. Since UnityPackages cannot
+// delete files, only replace them, this message is left here to prevent old
+// files from causing compiler errors
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtree.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtree.cs.meta
new file mode 100644
index 0000000..f18d4ad
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtree.cs.meta
@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 4fadcced8ad4d40d59f3eea45426359d
+MonoImporter:
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtreeBurst.cs b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtreeBurst.cs
new file mode 100644
index 0000000..f0222dc
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtreeBurst.cs
@@ -0,0 +1,698 @@
+namespace Pathfinding.RVO {
+	using UnityEngine;
+	using Pathfinding.ECS.RVO;
+	using Unity.Burst;
+	using Unity.Jobs;
+	using Unity.Mathematics;
+	using Unity.Collections;
+	using Pathfinding.Drawing;
+
+	/// <summary>
+	/// Quadtree for quick nearest neighbour search of rvo agents.
+	/// See: Pathfinding.RVO.Simulator
+	/// </summary>
+	public struct RVOQuadtreeBurst {
+		const int LeafSize = 16;
+		const int MaxDepth = 10;
+
+		NativeArray<int> agents;
+		NativeArray<int> childPointers;
+		NativeArray<float3> boundingBoxBuffer;
+		NativeArray<int> agentCountBuffer;
+		NativeArray<float3> agentPositions;
+		NativeArray<float> agentRadii;
+		NativeArray<float> maxSpeeds;
+		NativeArray<float> maxRadius;
+		NativeArray<float> nodeAreas;
+		MovementPlane movementPlane;
+
+		const int LeafNodeBit = 1 << 30;
+		const int BitPackingShift = 15;
+		const int BitPackingMask = (1 << BitPackingShift) - 1;
+		const int MaxAgents = BitPackingMask;
+
+		/// <summary>
+		/// For a given number, contains the index of the first non-zero bit.
+		/// Only the values 0 through 15 are used when movementPlane is XZ or XY.
+		///
+		/// Use bytes instead of ints to save some precious L1 cache memory.
+		/// </summary>
+		static readonly byte[] ChildLookup = new byte[256];
+
+		static RVOQuadtreeBurst() {
+			for (int v = 0; v < 256; v++) {
+				for (int i = 0; i < 8; i++) {
+					if (((v >> i) & 0x1) != 0) {
+						ChildLookup[v] = (byte)i;
+						break;
+					}
+				}
+			}
+		}
+
+		public Rect bounds {
+			get {
+				return boundingBoxBuffer.IsCreated ? Rect.MinMaxRect(boundingBoxBuffer[0].x, boundingBoxBuffer[0].y, boundingBoxBuffer[1].x, boundingBoxBuffer[1].y) : new Rect();
+			}
+		}
+
+		static int InnerNodeCountUpperBound (int numAgents, MovementPlane movementPlane) {
+			// Every LeafSize number of nodes can cause a split at most MaxDepth
+			// number of times. Each split needs 4 (or 8) units of space.
+			// Round the value up by adding LeafSize-1 to the numerator.
+			// This is an upper bound. Most likely the tree will contain significantly fewer nodes.
+			return ((movementPlane == MovementPlane.Arbitrary ? 8 : 4) * MaxDepth * numAgents + LeafSize-1)/LeafSize;
+		}
+
+		public void Dispose () {
+			agents.Dispose();
+			childPointers.Dispose();
+			boundingBoxBuffer.Dispose();
+			agentCountBuffer.Dispose();
+			maxSpeeds.Dispose();
+			maxRadius.Dispose();
+			nodeAreas.Dispose();
+			agentPositions.Dispose();
+			agentRadii.Dispose();
+		}
+
+		void Reserve (int minSize) {
+			if (!boundingBoxBuffer.IsCreated) {
+				boundingBoxBuffer = new NativeArray<float3>(4, Allocator.Persistent);
+				agentCountBuffer = new NativeArray<int>(1, Allocator.Persistent);
+			}
+			// Create a new agent's array. Round up to nearest multiple multiple of 2 to avoid re-allocating often if the agent count slowly increases
+			int roundedAgents = math.ceilpow2(minSize);
+			Util.Memory.Realloc(ref agents, roundedAgents, Allocator.Persistent, NativeArrayOptions.ClearMemory);
+			Util.Memory.Realloc(ref agentPositions, roundedAgents, Allocator.Persistent, NativeArrayOptions.ClearMemory);
+			Util.Memory.Realloc(ref agentRadii, roundedAgents, Allocator.Persistent, NativeArrayOptions.ClearMemory);
+			Util.Memory.Realloc(ref childPointers, InnerNodeCountUpperBound(roundedAgents, movementPlane), Allocator.Persistent, NativeArrayOptions.ClearMemory);
+			Util.Memory.Realloc(ref maxSpeeds, childPointers.Length, Allocator.Persistent, NativeArrayOptions.ClearMemory);
+			Util.Memory.Realloc(ref nodeAreas, childPointers.Length, Allocator.Persistent, NativeArrayOptions.ClearMemory);
+			Util.Memory.Realloc(ref maxRadius, childPointers.Length, Allocator.Persistent, NativeArrayOptions.ClearMemory);
+		}
+
+		public JobBuild BuildJob (NativeArray<float3> agentPositions, NativeArray<AgentIndex> agentVersions, NativeArray<float> agentSpeeds, NativeArray<float> agentRadii, int numAgents, MovementPlane movementPlane) {
+			if (numAgents >= MaxAgents) throw new System.Exception("Too many agents. Cannot have more than " + MaxAgents);
+			Reserve(numAgents);
+
+			this.movementPlane = movementPlane;
+
+			return new JobBuild {
+					   agents = agents,
+					   agentVersions = agentVersions,
+					   agentPositions = agentPositions,
+					   agentSpeeds = agentSpeeds,
+					   agentRadii = agentRadii,
+					   outMaxSpeeds = maxSpeeds,
+					   outMaxRadius = maxRadius,
+					   outArea = nodeAreas,
+					   outAgentRadii = this.agentRadii, // Will be copied. These are copied so that the quadtree remains in a valid state even after new agents have been added/removed. This is important for the QueryArea method which may be called at any time.
+					   outAgentPositions = this.agentPositions, // Will be copied
+					   outBoundingBox = boundingBoxBuffer,
+					   outAgentCount = agentCountBuffer,
+					   outChildPointers = childPointers,
+					   numAgents = numAgents,
+					   movementPlane = movementPlane,
+			};
+		}
+
+		[BurstCompile(CompileSynchronously = true, FloatMode = FloatMode.Fast)]
+		public struct JobBuild : IJob {
+			/// <summary>Length should be greater or equal to agentPositions.Length</summary>
+			public NativeArray<int> agents;
+
+			[ReadOnly]
+			public NativeArray<float3> agentPositions;
+
+			[ReadOnly]
+			public NativeArray<AgentIndex> agentVersions;
+
+			[ReadOnly]
+			public NativeArray<float> agentSpeeds;
+
+			[ReadOnly]
+			public NativeArray<float> agentRadii;
+
+			/// <summary>Should have size 2</summary>
+			[WriteOnly]
+			public NativeArray<float3> outBoundingBox;
+
+			/// <summary>Should have size 1</summary>
+			[WriteOnly]
+			public NativeArray<int> outAgentCount;
+
+			/// <summary>Should have size: InnerNodeCountUpperBound(numAgents)</summary>
+			public NativeArray<int> outChildPointers;
+
+			/// <summary>Should have size: InnerNodeCountUpperBound(numAgents)</summary>
+			public NativeArray<float> outMaxSpeeds;
+
+			/// <summary>Should have size: InnerNodeCountUpperBound(numAgents)</summary>
+			public NativeArray<float> outMaxRadius;
+
+			/// <summary>Should have size: InnerNodeCountUpperBound(numAgents)</summary>
+			public NativeArray<float> outArea;
+
+			[WriteOnly]
+			public NativeArray<float3> outAgentPositions;
+
+			[WriteOnly]
+			public NativeArray<float> outAgentRadii;
+
+			public int numAgents;
+
+			public MovementPlane movementPlane;
+
+			static int Partition (NativeSlice<int> indices, int startIndex, int endIndex, NativeSlice<float> coordinates, float splitPoint) {
+				for (int i = startIndex; i < endIndex; i++) {
+					if (coordinates[indices[i]] > splitPoint) {
+						endIndex--;
+						var tmp = indices[i];
+						indices[i] = indices[endIndex];
+						indices[endIndex] = tmp;
+						i--;
+					}
+				}
+				return endIndex;
+			}
+
+			void BuildNode (float3 boundsMin, float3 boundsMax, int depth, int agentsStart, int agentsEnd, int nodeOffset, ref int firstFreeChild) {
+				if (agentsEnd - agentsStart > LeafSize && depth < MaxDepth) {
+					if (movementPlane == MovementPlane.Arbitrary) {
+						// Split the node into 8 equally sized (by volume) child nodes
+						var xs = new NativeSlice<float3>(agentPositions).SliceWithStride<float>(0);
+						var ys = new NativeSlice<float3>(agentPositions).SliceWithStride<float>(4);
+						var zs = new NativeSlice<float3>(agentPositions).SliceWithStride<float>(8);
+
+						float3 boundsMid = (boundsMin + boundsMax) * 0.5f;
+						int s0 = agentsStart;
+						int s8 = agentsEnd;
+						int s4 = Partition(agents, s0, s8, xs, boundsMid.x);
+						int s2 = Partition(agents, s0, s4, ys, boundsMid.y);
+						int s6 = Partition(agents, s4, s8, ys, boundsMid.y);
+						int s1 = Partition(agents, s0, s2, zs, boundsMid.z);
+						int s3 = Partition(agents, s2, s4, zs, boundsMid.z);
+						int s5 = Partition(agents, s4, s6, zs, boundsMid.z);
+						int s7 = Partition(agents, s6, s8, zs, boundsMid.z);
+
+						// Note: guaranteed to be large enough
+						int childIndex = firstFreeChild;
+						outChildPointers[nodeOffset] = childIndex;
+						firstFreeChild += 8;
+
+						// x    y     z
+						// low  low  low
+						// low  low  high
+						// low  high low
+						// low  high high
+						// high low  low
+						// high low  high
+						// high high low
+						// high high high
+						var min = boundsMin;
+						var mid = boundsMid;
+						var max = boundsMax;
+						BuildNode(new float3(min.x, min.y, min.z), new float3(mid.x, mid.y, mid.z), depth + 1, s0, s1, childIndex + 0, ref firstFreeChild);
+						BuildNode(new float3(min.x, min.y, mid.z), new float3(mid.x, mid.y, max.z), depth + 1, s1, s2, childIndex + 1, ref firstFreeChild);
+						BuildNode(new float3(min.x, mid.y, min.z), new float3(mid.x, max.y, mid.z), depth + 1, s2, s3, childIndex + 2, ref firstFreeChild);
+						BuildNode(new float3(min.x, mid.y, mid.z), new float3(mid.x, max.y, max.z), depth + 1, s3, s4, childIndex + 3, ref firstFreeChild);
+						BuildNode(new float3(mid.x, min.y, min.z), new float3(max.x, mid.y, mid.z), depth + 1, s4, s5, childIndex + 4, ref firstFreeChild);
+						BuildNode(new float3(mid.x, min.y, mid.z), new float3(max.x, mid.y, max.z), depth + 1, s5, s6, childIndex + 5, ref firstFreeChild);
+						BuildNode(new float3(mid.x, mid.y, min.z), new float3(max.x, max.y, mid.z), depth + 1, s6, s7, childIndex + 6, ref firstFreeChild);
+						BuildNode(new float3(mid.x, mid.y, mid.z), new float3(max.x, max.y, max.z), depth + 1, s7, s8, childIndex + 7, ref firstFreeChild);
+					} else if (movementPlane == MovementPlane.XY) {
+						// Split the node into 4 equally sized (by area) child nodes
+						var xs = new NativeSlice<float3>(agentPositions).SliceWithStride<float>(0);
+						var ys = new NativeSlice<float3>(agentPositions).SliceWithStride<float>(4);
+
+						float3 boundsMid = (boundsMin + boundsMax) * 0.5f;
+						int s0 = agentsStart;
+						int s4 = agentsEnd;
+						int s2 = Partition(agents, s0, s4, xs, boundsMid.x);
+						int s1 = Partition(agents, s0, s2, ys, boundsMid.y);
+						int s3 = Partition(agents, s2, s4, ys, boundsMid.y);
+
+						// Note: guaranteed to be large enough
+						int childIndex = firstFreeChild;
+						outChildPointers[nodeOffset] = childIndex;
+						firstFreeChild += 4;
+
+						// x    y
+						// low  low
+						// low  high
+						// high low
+						// high high
+						BuildNode(new float3(boundsMin.x, boundsMin.y, boundsMin.z), new float3(boundsMid.x, boundsMid.y, boundsMax.z), depth + 1, s0, s1, childIndex + 0, ref firstFreeChild);
+						BuildNode(new float3(boundsMin.x, boundsMid.y, boundsMin.z), new float3(boundsMid.x, boundsMax.y, boundsMax.z), depth + 1, s1, s2, childIndex + 1, ref firstFreeChild);
+						BuildNode(new float3(boundsMid.x, boundsMin.y, boundsMin.z), new float3(boundsMax.x, boundsMid.y, boundsMax.z), depth + 1, s2, s3, childIndex + 2, ref firstFreeChild);
+						BuildNode(new float3(boundsMid.x, boundsMid.y, boundsMin.z), new float3(boundsMax.x, boundsMax.y, boundsMax.z), depth + 1, s3, s4, childIndex + 3, ref firstFreeChild);
+					} else {
+						// Split the node into 4 equally sized (by area) child nodes
+						var xs = new NativeSlice<float3>(agentPositions).SliceWithStride<float>(0);
+						var zs = new NativeSlice<float3>(agentPositions).SliceWithStride<float>(8);
+
+						float3 boundsMid = (boundsMin + boundsMax) * 0.5f;
+						int s0 = agentsStart;
+						int s4 = agentsEnd;
+						int s2 = Partition(agents, s0, s4, xs, boundsMid.x);
+						int s1 = Partition(agents, s0, s2, zs, boundsMid.z);
+						int s3 = Partition(agents, s2, s4, zs, boundsMid.z);
+
+						// Note: guaranteed to be large enough
+						int childIndex = firstFreeChild;
+						outChildPointers[nodeOffset] = childIndex;
+						firstFreeChild += 4;
+
+						// x    z
+						// low  low
+						// low  high
+						// high low
+						// high high
+						BuildNode(new float3(boundsMin.x, boundsMin.y, boundsMin.z), new float3(boundsMid.x, boundsMax.y, boundsMid.z), depth + 1, s0, s1, childIndex + 0, ref firstFreeChild);
+						BuildNode(new float3(boundsMin.x, boundsMin.y, boundsMid.z), new float3(boundsMid.x, boundsMax.y, boundsMax.z), depth + 1, s1, s2, childIndex + 1, ref firstFreeChild);
+						BuildNode(new float3(boundsMid.x, boundsMin.y, boundsMin.z), new float3(boundsMax.x, boundsMax.y, boundsMid.z), depth + 1, s2, s3, childIndex + 2, ref firstFreeChild);
+						BuildNode(new float3(boundsMid.x, boundsMin.y, boundsMid.z), new float3(boundsMax.x, boundsMax.y, boundsMax.z), depth + 1, s3, s4, childIndex + 3, ref firstFreeChild);
+					}
+				} else {
+					// Bitpack the start and end indices
+					outChildPointers[nodeOffset] = agentsStart | (agentsEnd << BitPackingShift) | LeafNodeBit;
+				}
+			}
+
+			void CalculateSpeeds (int nodeCount) {
+				for (int i = nodeCount - 1; i >= 0; i--) {
+					if ((outChildPointers[i] & LeafNodeBit) != 0) {
+						int startIndex = outChildPointers[i] & BitPackingMask;
+						int endIndex = (outChildPointers[i] >> BitPackingShift) & BitPackingMask;
+						float speed = 0;
+						for (int j = startIndex; j < endIndex; j++) speed = math.max(speed, agentSpeeds[agents[j]]);
+						outMaxSpeeds[i] = speed;
+
+						float radius = 0;
+						for (int j = startIndex; j < endIndex; j++) radius = math.max(radius, agentRadii[agents[j]]);
+						outMaxRadius[i] = radius;
+
+						float area = 0;
+						for (int j = startIndex; j < endIndex; j++) area += agentRadii[agents[j]]*agentRadii[agents[j]];
+						outArea[i] = area;
+					} else {
+						// Take the maximum of all child speeds
+						// This is guaranteed to have been calculated already because we do the loop in reverse and child indices are always greater than the current index
+						int childIndex = outChildPointers[i];
+						if (movementPlane == MovementPlane.Arbitrary) {
+							// 8 children
+							float maxSpeed = 0;
+							float maxRadius = 0;
+							float area = 0;
+							for (int j = 0; j < 8; j++) {
+								maxSpeed = math.max(maxSpeed, outMaxSpeeds[childIndex + j]);
+								maxRadius = math.max(maxRadius, outMaxSpeeds[childIndex + j]);
+								area += outArea[childIndex + j];
+							}
+							outMaxSpeeds[i] = maxSpeed;
+							outMaxRadius[i] = maxRadius;
+							outArea[i] = area;
+						} else {
+							// 4 children
+							outMaxSpeeds[i] = math.max(math.max(outMaxSpeeds[childIndex], outMaxSpeeds[childIndex+1]), math.max(outMaxSpeeds[childIndex+2], outMaxSpeeds[childIndex+3]));
+							outMaxRadius[i] = math.max(math.max(outMaxRadius[childIndex], outMaxRadius[childIndex+1]), math.max(outMaxRadius[childIndex+2], outMaxRadius[childIndex+3]));
+
+							// Sum of child areas
+							outArea[i] = outArea[childIndex] + outArea[childIndex+1] + outArea[childIndex+2] + outArea[childIndex+3];
+						}
+					}
+				}
+			}
+
+			public void Execute () {
+				float3 mn = float.PositiveInfinity;
+				float3 mx = float.NegativeInfinity;
+				int existingAgentCount = 0;
+				for (int i = 0; i < numAgents; i++) {
+					if (agentVersions[i].Valid) {
+						agents[existingAgentCount++] = i;
+						mn = math.min(mn, agentPositions[i]);
+						mx = math.max(mx, agentPositions[i]);
+					}
+				}
+
+				outAgentCount[0] = existingAgentCount;
+
+				if (existingAgentCount == 0) {
+					outBoundingBox[0] = outBoundingBox[1] = float3.zero;
+					return;
+				}
+
+				outBoundingBox[0] = mn;
+				outBoundingBox[1] = mx;
+
+				int firstFreeChild = 1;
+				BuildNode(mn, mx, 0, 0, existingAgentCount, 0, ref firstFreeChild);
+
+				CalculateSpeeds(firstFreeChild);
+
+				NativeArray<float3>.Copy(agentPositions, outAgentPositions, numAgents);
+				NativeArray<float>.Copy(agentRadii, outAgentRadii, numAgents);
+			}
+		}
+
+		public struct QuadtreeQuery {
+			public float3 position;
+			public float speed, timeHorizon, agentRadius;
+			public int outputStartIndex, maxCount;
+			public NativeArray<int> result;
+			public NativeArray<float> resultDistances;
+		}
+
+		public void QueryKNearest (QuadtreeQuery query) {
+			if (!agents.IsCreated) return;
+			float maxRadius = float.PositiveInfinity;
+
+			for (int i = 0; i < query.maxCount; i++) query.result[query.outputStartIndex + i] = -1;
+			for (int i = 0; i < query.maxCount; i++) query.resultDistances[i] = float.PositiveInfinity;
+
+			QueryRec(ref query, 0, boundingBoxBuffer[0], boundingBoxBuffer[1], ref maxRadius);
+		}
+
+		void QueryRec (ref QuadtreeQuery query, int treeNodeIndex, float3 nodeMin, float3 nodeMax, ref float maxRadius) {
+			// Note: the second agentRadius usage should actually be the radius of the other agents, not this agent
+			// Determine the radius that we need to search to take all agents into account
+			// but for performance reasons and for simplicity we assume that agents have approximately the same radius.
+			// Thus an agent with a very small radius may in some cases detect an agent with a very large radius too late
+			// however this effect should be minor.
+			var radius = math.min(math.max((maxSpeeds[treeNodeIndex] + query.speed)*query.timeHorizon, query.agentRadius) + query.agentRadius, maxRadius);
+			float3 p = query.position;
+
+			if ((childPointers[treeNodeIndex] & LeafNodeBit) != 0) {
+				// Leaf node
+				int maxCount = query.maxCount;
+				int startIndex = childPointers[treeNodeIndex] & BitPackingMask;
+				int endIndex = (childPointers[treeNodeIndex] >> BitPackingShift) & BitPackingMask;
+
+				var result = query.result;
+				var resultDistances = query.resultDistances;
+				for (int j = startIndex; j < endIndex; j++) {
+					var agent = agents[j];
+					float sqrDistance = math.lengthsq(p - agentPositions[agent]);
+					if (sqrDistance < radius*radius) {
+						// Close enough
+
+						// Insert the agent into the results list using insertion sort
+						for (int k = 0; k < maxCount; k++) {
+							if (sqrDistance < resultDistances[k]) {
+								// Move the remaining items one step in the array
+								for (int q = maxCount - 1; q > k; q--) {
+									result[query.outputStartIndex + q] = result[query.outputStartIndex + q-1];
+									resultDistances[q] = resultDistances[q-1];
+								}
+								result[query.outputStartIndex + k] = agent;
+								resultDistances[k] = sqrDistance;
+
+								if (k == maxCount - 1) {
+									// We reached the end of the array. This means that we just updated the largest distance.
+									// We can use this to restrict the future search. We know that no other agent distance we find can be larger than this value.
+									maxRadius = math.min(maxRadius, math.sqrt(sqrDistance));
+									radius = math.min(radius, maxRadius);
+								}
+								break;
+							}
+						}
+					}
+				}
+			} else {
+				// Not a leaf node
+				int childrenStartIndex = childPointers[treeNodeIndex];
+
+				float3 nodeMid = (nodeMin + nodeMax) * 0.5f;
+				if (movementPlane == MovementPlane.Arbitrary) {
+					// First visit the child that overlaps the query position.
+					// This is important to do first as it usually reduces the maxRadius significantly
+					// and thus reduces the number of children we have to search later.
+					var mainChildIndex = (p.x < nodeMid.x ? 0 : 4) | (p.y < nodeMid.y ? 0 : 2) | (p.z < nodeMid.z ? 0 : 1);
+					{
+						var selector = new bool3((mainChildIndex & 0x4) != 0, (mainChildIndex & 0x2) != 0, (mainChildIndex & 0x1) != 0);
+
+						var mn = math.select(nodeMin, nodeMid, selector);
+						var mx = math.select(nodeMid, nodeMax, selector);
+						QueryRec(ref query, childrenStartIndex + mainChildIndex, mn, mx, ref maxRadius);
+						radius = math.min(radius, maxRadius);
+					}
+
+					// Visit a child if a cube with sides of length 2*radius (centered at p) touches the child.
+					// We calculate this info for all 8 children at the same time.
+					// Each child contains three checks, one for each axis.
+					// For example for the child which is lower than mid on the x-axis and z-axis, but higher than mid on the y axis
+					// the check we want to do looks like: (p.x - radius < nodeMid.x && p.y + radius > nodeMid.y && p.z - radius < nodeMid.z)
+					var lessThanMid = p - radius < nodeMid;
+					var greaterThanMid = p + radius > nodeMid;
+					// If for example lessThanMid.x is false, then we can exclude all 4 children that require that check
+					var branch1 = math.select(new int3(0b11110000, 0b11001100, 0b10101010), new int3(0xFF, 0xFF, 0xFF), lessThanMid);
+					var branch2 = math.select(new int3(0b00001111, 0b00110011, 0b01010101), new int3(0xFF, 0xFF, 0xFF), greaterThanMid);
+					var toVisitByAxis = branch1 & branch2;
+					// Combine the checks for each axis
+					// Bitmask of which children we want to visit (1 = visit, 0 = don't visit)
+					var childrenToVisit = toVisitByAxis.x & toVisitByAxis.y & toVisitByAxis.z;
+
+					childrenToVisit &= ~(1 << mainChildIndex);
+
+					// Loop over all children that we will visit.
+					// It's nice with a loop because we will usually only have a single branch.
+					while (childrenToVisit != 0) {
+						var childIndex = ChildLookup[childrenToVisit];
+						var selector = new bool3((childIndex & 0x4) != 0, (childIndex & 0x2) != 0, (childIndex & 0x1) != 0);
+
+						var mn = math.select(nodeMin, nodeMid, selector);
+						var mx = math.select(nodeMid, nodeMax, selector);
+						QueryRec(ref query, childrenStartIndex + childIndex, mn, mx, ref maxRadius);
+						radius = math.min(radius, maxRadius);
+						childrenToVisit &= ~(1 << childIndex);
+					}
+				} else if (movementPlane == MovementPlane.XY) {
+					var mainChildIndex = (p.x < nodeMid.x ? 0 : 2) | (p.y < nodeMid.y ? 0 : 1);
+					{
+						// Note: mx.z will become nodeMid.z which is technically incorrect, but we don't care about the Z coordinate here anyway
+						var selector = new bool3((mainChildIndex & 0x2) != 0, (mainChildIndex & 0x1) != 0, false);
+
+						var mn = math.select(nodeMin, nodeMid, selector);
+						var mx = math.select(nodeMid, nodeMax, selector);
+						QueryRec(ref query, childrenStartIndex + mainChildIndex, mn, mx, ref maxRadius);
+						radius = math.min(radius, maxRadius);
+					}
+
+					var lessThanMid = p.xy - radius < nodeMid.xy;
+					var greaterThanMid = p.xy + radius > nodeMid.xy;
+
+					var v = new bool4(lessThanMid.x & lessThanMid.y, lessThanMid.x & greaterThanMid.y, greaterThanMid.x & lessThanMid.y, greaterThanMid.x & greaterThanMid.y);
+					// Build a bitmask of which children to visit
+					var childrenToVisit = (v.x ? 1 : 0) | (v.y ? 2 : 0) | (v.z ? 4 : 0) | (v.w ? 8 : 0);
+					childrenToVisit &= ~(1 << mainChildIndex);
+
+					// Loop over all children that we will visit.
+					// It's nice with a loop because we will usually only have a single branch.
+					while (childrenToVisit != 0) {
+						var childIndex = ChildLookup[childrenToVisit];
+						// Note: mx.z will become nodeMid.z which is technically incorrect, but we don't care about the Z coordinate here anyway
+						var selector = new bool3((childIndex & 0x2) != 0, (childIndex & 0x1) != 0, false);
+
+						var mn = math.select(nodeMin, nodeMid, selector);
+						var mx = math.select(nodeMid, nodeMax, selector);
+						QueryRec(ref query, childrenStartIndex + childIndex, mn, mx, ref maxRadius);
+						radius = math.min(radius, maxRadius);
+						childrenToVisit &= ~(1 << childIndex);
+					}
+				} else {
+					var mainChildIndex = (p.x < nodeMid.x ? 0 : 2) | (p.z < nodeMid.z ? 0 : 1);
+					{
+						// Note: mx.y will become nodeMid.y which is technically incorrect, but we don't care about the Y coordinate here anyway
+						var selector = new bool3((mainChildIndex & 0x2) != 0, false, (mainChildIndex & 0x1) != 0);
+
+						var mn = math.select(nodeMin, nodeMid, selector);
+						var mx = math.select(nodeMid, nodeMax, selector);
+						QueryRec(ref query, childrenStartIndex + mainChildIndex, mn, mx, ref maxRadius);
+						radius = math.min(radius, maxRadius);
+					}
+
+					var lessThanMid = p.xz - radius < nodeMid.xz;
+					var greaterThanMid = p.xz + radius > nodeMid.xz;
+
+					var v = new bool4(lessThanMid.x & lessThanMid.y, lessThanMid.x & greaterThanMid.y, greaterThanMid.x & lessThanMid.y, greaterThanMid.x & greaterThanMid.y);
+					var childrenToVisit = (v.x ? 1 : 0) | (v.y ? 2 : 0) | (v.z ? 4 : 0) | (v.w ? 8 : 0);
+					childrenToVisit &= ~(1 << mainChildIndex);
+
+					while (childrenToVisit != 0) {
+						var childIndex = ChildLookup[childrenToVisit];
+						// Note: mx.y will become nodeMid.y which is technically incorrect, but we don't care about the Y coordinate here anyway
+						var selector = new bool3((childIndex & 0x2) != 0, false, (childIndex & 0x1) != 0);
+
+						var mn = math.select(nodeMin, nodeMid, selector);
+						var mx = math.select(nodeMid, nodeMax, selector);
+						QueryRec(ref query, childrenStartIndex + childIndex, mn, mx, ref maxRadius);
+						radius = math.min(radius, maxRadius);
+						childrenToVisit &= ~(1 << childIndex);
+					}
+				}
+			}
+		}
+
+		/// <summary>Find the total agent area inside the circle at position with the given radius</summary>
+		public float QueryArea (float3 position, float radius) {
+			if (!agents.IsCreated || agentCountBuffer[0] == 0) return 0f;
+			return math.PI * QueryAreaRec(0, position, radius, boundingBoxBuffer[0], boundingBoxBuffer[1]);
+		}
+
+		float QueryAreaRec (int treeNodeIndex, float3 p, float radius, float3 nodeMin, float3 nodeMax) {
+			float3 nodeMid = (nodeMin + nodeMax) * 0.5f;
+			// Radius of a circle that is guaranteed to cover the entire node
+			float nodeRadius = math.length(nodeMax - nodeMid);
+			float dist = math.lengthsq(nodeMid - p);
+			var maxAgentRadius = maxRadius[treeNodeIndex];
+			var thresholdDistance = radius - (nodeRadius + maxAgentRadius);
+
+			if (thresholdDistance > 0 && dist < thresholdDistance*thresholdDistance) {
+				// Node is completely inside the circle. Return the precalculated area of all agents inside the node.
+				return nodeAreas[treeNodeIndex];
+			}
+
+			if (dist > (radius + (nodeRadius + maxAgentRadius))*(radius + (nodeRadius + maxAgentRadius))) {
+				return 0;
+			}
+
+			if ((childPointers[treeNodeIndex] & LeafNodeBit) != 0) {
+				// Leaf node
+				// Node is partially inside the circle
+
+				int startIndex = childPointers[treeNodeIndex] & BitPackingMask;
+				int endIndex = (childPointers[treeNodeIndex] >> BitPackingShift) & BitPackingMask;
+
+				float k = 0;
+				float area = 0;
+				for (int j = startIndex; j < endIndex; j++) {
+					var agent = agents[j];
+					k += agentRadii[agent]*agentRadii[agent];
+					float sqrDistance = math.lengthsq(p - agentPositions[agent]);
+					float agentRadius = agentRadii[agent];
+					if (sqrDistance < (radius + agentRadius)*(radius + agentRadius)) {
+						float innerRadius = radius - agentRadius;
+						// Slight approximation at the edge of the circle.
+						// This is the approximate fraction of the agent that is inside the circle.
+						float fractionInside = sqrDistance < innerRadius*innerRadius ? 1.0f : 1.0f - (math.sqrt(sqrDistance) - innerRadius) / (2*agentRadius);
+						area += agentRadius*agentRadius * fractionInside;
+					}
+				}
+				return area;
+			} else {
+				float area = 0;
+				// Not a leaf node
+				int childIndex = childPointers[treeNodeIndex];
+				float radiusWithMargin = radius + maxAgentRadius;
+				if (movementPlane == MovementPlane.Arbitrary) {
+					bool3 lower = (p - radiusWithMargin) < nodeMid;
+					bool3 upper = (p + radiusWithMargin) > nodeMid;
+					if (lower[0]) {
+						if (lower[1]) {
+							if (lower[2]) area += QueryAreaRec(childIndex + 0, p, radius, new float3(nodeMin.x, nodeMin.y, nodeMin.z), new float3(nodeMid.x, nodeMid.y, nodeMid.z));
+							if (upper[2]) area += QueryAreaRec(childIndex + 1, p, radius, new float3(nodeMin.x, nodeMin.y, nodeMid.z), new float3(nodeMid.x, nodeMid.y, nodeMax.z));
+						}
+						if (upper[1]) {
+							if (lower[2]) area += QueryAreaRec(childIndex + 2, p, radius, new float3(nodeMin.x, nodeMid.y, nodeMin.z), new float3(nodeMid.x, nodeMax.y, nodeMid.z));
+							if (upper[2]) area += QueryAreaRec(childIndex + 3, p, radius, new float3(nodeMin.x, nodeMid.y, nodeMid.z), new float3(nodeMid.x, nodeMax.y, nodeMax.z));
+						}
+					}
+					if (upper[0]) {
+						if (lower[1]) {
+							if (lower[2]) area += QueryAreaRec(childIndex + 4, p, radius, new float3(nodeMid.x, nodeMin.y, nodeMin.z), new float3(nodeMax.x, nodeMid.y, nodeMid.z));
+							if (upper[2]) area += QueryAreaRec(childIndex + 5, p, radius, new float3(nodeMid.x, nodeMin.y, nodeMid.z), new float3(nodeMax.x, nodeMid.y, nodeMax.z));
+						}
+						if (upper[1]) {
+							if (lower[2]) area += QueryAreaRec(childIndex + 6, p, radius, new float3(nodeMid.x, nodeMid.y, nodeMin.z), new float3(nodeMax.x, nodeMax.y, nodeMid.z));
+							if (upper[2]) area += QueryAreaRec(childIndex + 7, p, radius, new float3(nodeMid.x, nodeMid.y, nodeMid.z), new float3(nodeMax.x, nodeMax.y, nodeMax.z));
+						}
+					}
+				} else if (movementPlane == MovementPlane.XY) {
+					bool2 lower = (p - radiusWithMargin).xy < nodeMid.xy;
+					bool2 upper = (p + radiusWithMargin).xy > nodeMid.xy;
+					if (lower[0]) {
+						if (lower[1]) area += QueryAreaRec(childIndex + 0, p, radius, new float3(nodeMin.x, nodeMin.y, nodeMin.z), new float3(nodeMid.x, nodeMid.y, nodeMax.z));
+						if (upper[1]) area += QueryAreaRec(childIndex + 1, p, radius, new float3(nodeMin.x, nodeMid.y, nodeMin.z), new float3(nodeMid.x, nodeMax.y, nodeMax.z));
+					}
+					if (upper[0]) {
+						if (lower[1]) area += QueryAreaRec(childIndex + 2, p, radius, new float3(nodeMid.x, nodeMin.y, nodeMin.z), new float3(nodeMax.x, nodeMid.y, nodeMax.z));
+						if (upper[1]) area += QueryAreaRec(childIndex + 3, p, radius, new float3(nodeMid.x, nodeMid.y, nodeMin.z), new float3(nodeMax.x, nodeMax.y, nodeMax.z));
+					}
+				} else {
+					bool2 lower = (p - radiusWithMargin).xz < nodeMid.xz;
+					bool2 upper = (p + radiusWithMargin).xz > nodeMid.xz;
+					if (lower[0]) {
+						if (lower[1]) area += QueryAreaRec(childIndex + 0, p, radius, new float3(nodeMin.x, nodeMin.y, nodeMin.z), new float3(nodeMid.x, nodeMax.y, nodeMid.z));
+						if (upper[1]) area += QueryAreaRec(childIndex + 1, p, radius, new float3(nodeMin.x, nodeMin.y, nodeMid.z), new float3(nodeMid.x, nodeMax.y, nodeMax.z));
+					}
+					if (upper[0]) {
+						if (lower[1]) area += QueryAreaRec(childIndex + 2, p, radius, new float3(nodeMid.x, nodeMin.y, nodeMin.z), new float3(nodeMax.x, nodeMax.y, nodeMid.z));
+						if (upper[1]) area += QueryAreaRec(childIndex + 3, p, radius, new float3(nodeMid.x, nodeMin.y, nodeMid.z), new float3(nodeMax.x, nodeMax.y, nodeMax.z));
+					}
+				}
+				return area;
+			}
+		}
+
+		[BurstCompile]
+		public struct DebugDrawJob : IJob {
+			public CommandBuilder draw;
+			[ReadOnly]
+			public RVOQuadtreeBurst quadtree;
+
+			public void Execute () {
+				quadtree.DebugDraw(draw);
+			}
+		}
+
+		public void DebugDraw (CommandBuilder draw) {
+			if (!agentCountBuffer.IsCreated) return;
+			var numAgents = agentCountBuffer[0];
+			if (numAgents == 0) return;
+
+			DebugDraw(0, boundingBoxBuffer[0], boundingBoxBuffer[1], draw);
+			for (int i = 0; i < numAgents; i++) {
+				draw.Cross(agentPositions[agents[i]], 0.5f, Palette.Colorbrewer.Set1.Red);
+			}
+		}
+
+		void DebugDraw (int nodeIndex, float3 nodeMin, float3 nodeMax, CommandBuilder draw) {
+			float3 nodeMid = (nodeMin + nodeMax) * 0.5f;
+
+			draw.WireBox(nodeMid, nodeMax - nodeMin, Palette.Colorbrewer.Set1.Orange);
+
+			if ((childPointers[nodeIndex] & LeafNodeBit) != 0) {
+				int startIndex = childPointers[nodeIndex] & BitPackingMask;
+				int endIndex = (childPointers[nodeIndex] >> BitPackingShift) & BitPackingMask;
+
+				for (int j = startIndex; j < endIndex; j++) {
+					draw.Line(nodeMid, agentPositions[agents[j]], Color.black);
+				}
+			} else {
+				int childIndex = childPointers[nodeIndex];
+				if (movementPlane == MovementPlane.Arbitrary) {
+					DebugDraw(childIndex + 0, new float3(nodeMin.x, nodeMin.y, nodeMin.z), new float3(nodeMid.x, nodeMid.y, nodeMid.z), draw);
+					DebugDraw(childIndex + 1, new float3(nodeMin.x, nodeMin.y, nodeMid.z), new float3(nodeMid.x, nodeMid.y, nodeMax.z), draw);
+					DebugDraw(childIndex + 2, new float3(nodeMin.x, nodeMid.y, nodeMin.z), new float3(nodeMid.x, nodeMax.y, nodeMid.z), draw);
+					DebugDraw(childIndex + 3, new float3(nodeMin.x, nodeMid.y, nodeMid.z), new float3(nodeMid.x, nodeMax.y, nodeMax.z), draw);
+					DebugDraw(childIndex + 4, new float3(nodeMid.x, nodeMin.y, nodeMin.z), new float3(nodeMax.x, nodeMid.y, nodeMid.z), draw);
+					DebugDraw(childIndex + 5, new float3(nodeMid.x, nodeMin.y, nodeMid.z), new float3(nodeMax.x, nodeMid.y, nodeMax.z), draw);
+					DebugDraw(childIndex + 6, new float3(nodeMid.x, nodeMid.y, nodeMin.z), new float3(nodeMax.x, nodeMax.y, nodeMid.z), draw);
+					DebugDraw(childIndex + 7, new float3(nodeMid.x, nodeMid.y, nodeMid.z), new float3(nodeMax.x, nodeMax.y, nodeMax.z), draw);
+				} else if (movementPlane == MovementPlane.XY) {
+					DebugDraw(childIndex + 0, new float3(nodeMin.x, nodeMin.y, nodeMin.z), new float3(nodeMid.x, nodeMid.y, nodeMax.z), draw);
+					DebugDraw(childIndex + 1, new float3(nodeMin.x, nodeMid.y, nodeMin.z), new float3(nodeMid.x, nodeMax.y, nodeMax.z), draw);
+					DebugDraw(childIndex + 2, new float3(nodeMid.x, nodeMin.y, nodeMin.z), new float3(nodeMax.x, nodeMid.y, nodeMax.z), draw);
+					DebugDraw(childIndex + 3, new float3(nodeMid.x, nodeMid.y, nodeMin.z), new float3(nodeMax.x, nodeMax.y, nodeMax.z), draw);
+				} else {
+					DebugDraw(childIndex + 0, new float3(nodeMin.x, nodeMin.y, nodeMin.z), new float3(nodeMid.x, nodeMax.y, nodeMid.z), draw);
+					DebugDraw(childIndex + 1, new float3(nodeMin.x, nodeMin.y, nodeMid.z), new float3(nodeMid.x, nodeMax.y, nodeMax.z), draw);
+					DebugDraw(childIndex + 2, new float3(nodeMid.x, nodeMin.y, nodeMin.z), new float3(nodeMax.x, nodeMax.y, nodeMid.z), draw);
+					DebugDraw(childIndex + 3, new float3(nodeMid.x, nodeMin.y, nodeMid.z), new float3(nodeMax.x, nodeMax.y, nodeMax.z), draw);
+				}
+			}
+		}
+	}
+}
diff --git a/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtreeBurst.cs.meta b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtreeBurst.cs.meta
new file mode 100644
index 0000000..0482032
--- /dev/null
+++ b/Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOQuadtreeBurst.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 6a4ff665235b244238a6c694d7d38b83
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: