/// <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);
}
}
}
}
}
