protected override void Prepare(ref Unemployed job, float delta) { int agentCount = m_lockedAgents.Count; if (m_outputAgents.Length != agentCount) { m_outputAgents.Dispose(); m_outputAgents = new NativeArray <AgentData>(agentCount, Allocator.Persistent); } m_maxRadius = 0f; Agent a; float3 pos, prefVel, vel; if (plane == AxisPair.XY) { for (int i = 0; i < agentCount; i++) { a = m_lockedAgents[i]; m_maxRadius = max(m_maxRadius, a.radius); pos = a.pos; prefVel = a.m_prefVelocity; vel = a.m_velocity; m_outputAgents[i] = new AgentData() { index = i, kdIndex = i, position = float2(pos.x, pos.y), // worldPosition = pos, baseline = pos.z, prefVelocity = float2(prefVel.x, prefVel.y), velocity = float2(vel.x, vel.y), worldVelocity = vel, height = a.m_height, radius = a.m_radius, radiusObst = a.m_radiusObst, maxSpeed = a.m_maxSpeed, maxNeighbors = a.m_maxNeighbors, neighborDist = a.m_neighborDist, neighborElev = a.m_neighborElev, timeHorizon = a.m_timeHorizon, timeHorizonObst = a.m_timeHorizonObst, navigationEnabled = a.m_navigationEnabled, collisionEnabled = a.m_collisionEnabled, layerOccupation = a.m_layerOccupation, layerIgnore = a.m_layerIgnore }; } } else { for (int i = 0; i < agentCount; i++) { a = m_lockedAgents[i]; m_maxRadius = max(m_maxRadius, a.radius); pos = a.pos; prefVel = a.m_prefVelocity; vel = a.m_velocity; m_outputAgents[i] = new AgentData() { index = i, position = float2(pos.x, pos.z), // worldPosition = pos, baseline = pos.y, prefVelocity = float2(prefVel.x, prefVel.z), velocity = float2(vel.x, vel.z), worldVelocity = vel, height = a.m_height, radius = a.m_radius, radiusObst = a.m_radiusObst, maxSpeed = a.m_maxSpeed, maxNeighbors = a.m_maxNeighbors, neighborDist = a.m_neighborDist, neighborElev = a.m_neighborElev, timeHorizon = a.m_timeHorizon, timeHorizonObst = a.m_timeHorizonObst, navigationEnabled = a.m_navigationEnabled, collisionEnabled = a.m_collisionEnabled, layerOccupation = a.m_layerOccupation, layerIgnore = a.m_layerIgnore }; } } }
/// <summary> /// Recursive method for building an agent k-D tree. /// </summary> /// <param name="begin">The beginning agent k-D tree node node index.</param> /// <param name="end">The ending agent k-D tree node index.</param> /// <param name="node">The current agent k-D tree node index.</param> private void BuildAgentTreeRecursive(int begin, int end, int node) { AgentTreeNode treeNode = m_outputTree[node]; AgentData agent = m_inputAgents[begin]; float2 pos; treeNode.begin = begin; treeNode.end = end; treeNode.minX = treeNode.maxX = agent.position.x; treeNode.minY = treeNode.maxY = agent.position.y; for (int i = begin + 1; i < end; ++i) { pos = m_inputAgents[i].position; treeNode.maxX = max(treeNode.maxX, pos.x); treeNode.minX = min(treeNode.minX, pos.x); treeNode.maxY = max(treeNode.maxY, pos.y); treeNode.minY = min(treeNode.minY, pos.y); } m_outputTree[node] = treeNode; if (end - begin > AgentTreeNode.MAX_LEAF_SIZE) { // No leaf node. bool isVertical = treeNode.maxX - treeNode.minX > treeNode.maxY - treeNode.minY; float splitValue = 0.5f * (isVertical ? treeNode.maxX + treeNode.minX : treeNode.maxY + treeNode.minY); int left = begin; int right = end; while (left < right) { while (left < right && (isVertical ? m_inputAgents[left].position.x : m_inputAgents[left].position.y) < splitValue) { ++left; } while (right > left && (isVertical ? m_inputAgents[right - 1].position.x : m_inputAgents[right - 1].position.y) >= splitValue) { --right; } if (left < right) { AgentData tempAgent = m_inputAgents[left], rep = m_inputAgents[right - 1]; tempAgent.kdIndex = right - 1; rep.kdIndex = left; m_inputAgents[left] = rep; m_inputAgents[right - 1] = tempAgent; ++left; --right; } } int leftSize = left - begin; if (leftSize == 0) { ++leftSize; ++left; ++right; } treeNode.left = node + 1; treeNode.right = node + 2 * leftSize; m_outputTree[node] = treeNode; BuildAgentTreeRecursive(begin, left, treeNode.left); BuildAgentTreeRecursive(left, end, treeNode.right); } }