diff options
Diffstat (limited to 'Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgentBurst.cs')
| -rw-r--r-- | Other/AstarPathfindingDemo/Packages/com.arongranberg.astar/Core/RVO/RVOAgentBurst.cs | 1998 |
1 files changed, 1998 insertions, 0 deletions
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 + } + } +} |