/// <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); } }
/// <summary> /// Recursive method for computing the agent neighbors of the specified raycast. /// </summary> /// <param name="raycast">The agent for which agent neighbors are to be computed.</param> /// <param name="raycast">The agent making the initial query</param> /// <param name="rangeSq">The squared range around the agent.</param> /// <param name="node">The current agent k-D tree node index.</param> /// <param name="agentNeighbors">The list of neighbors to be filled up.</param> private void QueryAgentTreeRecursive(ref RaycastData raycast, ref float rangeSq, int node, ref NativeList <int> agentNeighbors) { float2 center = raycast.position; AgentTreeNode treeNode = m_inputAgentTree[node]; if (treeNode.end - treeNode.begin <= AgentTreeNode.MAX_LEAF_SIZE) { AgentData a; float bottom = raycast.baseline, top = bottom; for (int i = treeNode.begin; i < treeNode.end; ++i) { a = m_inputAgents[i]; if (!a.collisionEnabled || (a.layerOccupation & ~raycast.layerIgnore) == 0 || (top < a.baseline || bottom > a.baseline + a.height)) { continue; } //if ((distancesq(center, a.position) - lengthsq(a.radius)) < rangeSq) agentNeighbors.Add(i); } } else { AgentTreeNode leftNode = m_inputAgentTree[treeNode.left], rightNode = m_inputAgentTree[treeNode.right]; float distSqLeft = lengthsq(max(0.0f, leftNode.minX - center.x)) + lengthsq(max(0.0f, center.x - leftNode.maxX)) + lengthsq(max(0.0f, leftNode.minY - center.y)) + lengthsq(max(0.0f, center.y - leftNode.maxY)); float distSqRight = lengthsq(max(0.0f, rightNode.minX - center.x)) + lengthsq(max(0.0f, center.x - rightNode.maxX)) + lengthsq(max(0.0f, rightNode.minY - center.y)) + lengthsq(max(0.0f, center.y - rightNode.maxY)); if (distSqLeft < distSqRight) { if (distSqLeft < rangeSq) { QueryAgentTreeRecursive(ref raycast, ref rangeSq, treeNode.left, ref agentNeighbors); if (distSqRight < rangeSq) { QueryAgentTreeRecursive(ref raycast, ref rangeSq, treeNode.right, ref agentNeighbors); } } } else { if (distSqRight < rangeSq) { QueryAgentTreeRecursive(ref raycast, ref rangeSq, treeNode.right, ref agentNeighbors); if (distSqLeft < rangeSq) { QueryAgentTreeRecursive(ref raycast, ref rangeSq, treeNode.left, ref agentNeighbors); } } } } }