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