/// <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)
{
ObstacleTreeNode treeNode = m_outputTree[node];
ObstacleVertexData obstacle = m_inputObstacles[begin];
float2 pos;
float minX, minY, maxX, maxY;
treeNode.begin = begin;
treeNode.end = end;
minX = maxX = obstacle.pos.x;
minY = maxY = obstacle.pos.y;
for (int i = begin + 1; i < end; ++i)
{
pos = m_inputObstacles[i].pos;
maxX = max(maxX, pos.x);
minX = min(minX, pos.x);
maxY = max(maxY, pos.y);
minY = min(minY, pos.y);
}
treeNode.minX = minX;
treeNode.maxX = maxX;
treeNode.minY = minY;
treeNode.maxY = maxY;
m_outputTree[node] = treeNode;
if (end - begin > ObstacleTreeNode.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_inputObstacles[left].pos.x : m_inputObstacles[left].pos.y) < splitValue)
{
++left;
}
while (right > left && (isVertical ? m_inputObstacles[right - 1].pos.x : m_inputObstacles[right - 1].pos.y) >= splitValue)
{
--right;
}
if (left < right)
{
ObstacleVertexData tempAgent = m_inputObstacles[left];
m_inputObstacles[left] = m_inputObstacles[right - 1];
m_inputObstacles[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);
}
}
private void QueryObstacleTreeRecursive(
ref RaycastData raycast,
ref float rangeSq,
int node,
ref NativeList <int> obstacleNeighbors,
ref NativeArray <ObstacleVertexData> obstacles,
ref NativeArray <ObstacleVertexData> refObstacles,
ref NativeArray <ObstacleInfos> obstaclesInfos,
ref NativeArray <ObstacleTreeNode> kdTree)
{
float2 center = raycast.position;
ObstacleTreeNode treeNode = kdTree[node];
if (treeNode.end - treeNode.begin <= ObstacleTreeNode.MAX_LEAF_SIZE)
{
ObstacleVertexData o;
ObstacleInfos infos;
float top = raycast.baseline, bottom = raycast.baseline;
float2 oPos, nPos;
for (int i = treeNode.begin; i < treeNode.end; ++i)
{
o = obstacles[i];
infos = obstaclesInfos[o.infos];
if (!infos.collisionEnabled || (infos.layerOccupation & ~raycast.layerIgnore) == 0)
{
continue;
}
if (top < infos.baseline || bottom > infos.baseline + infos.height)
{
continue;
}
oPos = o.pos; nPos = refObstacles[o.next].pos;
float distSq = DistSqPointLineSegment(oPos, nPos, center);
if (distSq < rangeSq)
{
float raycastLeftOfLine = LeftOf(oPos, nPos, center);
if ((lengthsq(raycastLeftOfLine) / lengthsq(nPos - oPos)) < rangeSq && raycastLeftOfLine < 0.0f)
{
obstacleNeighbors.Add(i);
}
}
}
}
else
{
ObstacleTreeNode leftNode = kdTree[treeNode.left],
rightNode = kdTree[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)
{
QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.left, ref obstacleNeighbors,
ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);
if (distSqRight < rangeSq)
{
QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.right, ref obstacleNeighbors,
ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);
}
}
}
else
{
if (distSqRight < rangeSq)
{
QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.right, ref obstacleNeighbors,
ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);
if (distSqLeft < rangeSq)
{
QueryObstacleTreeRecursive(ref raycast, ref rangeSq, treeNode.left, ref obstacleNeighbors,
ref obstacles, ref refObstacles, ref obstaclesInfos, ref kdTree);
}
}
}
}
}
