public void Query(TSVector p, FP speed, FP timeHorizon, FP TRadius, T T)
{
QuadtreeQuery tmp = new QuadtreeQuery();
tmp.p = p;
tmp.speed = speed;
tmp.timeHorizon = timeHorizon;
tmp.maxRadius = FP.One * 1000000;//avoid overflow
tmp.TRadius = TRadius;
tmp.T = T;
tmp.nodes = nodes;
tmp.QueryRec(0, bounds);
}
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 childIndex = childPointers[treeNodeIndex];
float3 nodeMid = (nodeMin + nodeMax) * 0.5f;
if (movementPlane == MovementPlane.Arbitrary)
{
if (p.x - radius < nodeMid.x)
{
if (p.y - radius < nodeMid.y)
{
if (p.z - radius < nodeMid.z)
{
QueryRec(ref query, childIndex + 0, new float3(nodeMin.x, nodeMin.y, nodeMin.z), new float3(nodeMid.x, nodeMid.y, nodeMid.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
if (p.z + radius > nodeMid.z)
{
QueryRec(ref query, childIndex + 1, new float3(nodeMin.x, nodeMin.y, nodeMid.z), new float3(nodeMid.x, nodeMid.y, nodeMax.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
}
if (p.y + radius > nodeMid.y)
{
if (p.z - radius < nodeMid.z)
{
QueryRec(ref query, childIndex + 2, new float3(nodeMin.x, nodeMid.y, nodeMin.z), new float3(nodeMid.x, nodeMax.y, nodeMid.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
if (p.z + radius > nodeMid.z)
{
QueryRec(ref query, childIndex + 3, new float3(nodeMin.x, nodeMid.y, nodeMid.z), new float3(nodeMid.x, nodeMax.y, nodeMax.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
}
}
if (p.x + radius > nodeMid.x)
{
if (p.y - radius < nodeMid.y)
{
if (p.z - radius < nodeMid.z)
{
QueryRec(ref query, childIndex + 4, new float3(nodeMid.x, nodeMin.y, nodeMin.z), new float3(nodeMax.x, nodeMid.y, nodeMid.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
if (p.z + radius > nodeMid.z)
{
QueryRec(ref query, childIndex + 5, new float3(nodeMid.x, nodeMin.y, nodeMid.z), new float3(nodeMax.x, nodeMid.y, nodeMax.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
}
if (p.y + radius > nodeMid.y)
{
if (p.z - radius < nodeMid.z)
{
QueryRec(ref query, childIndex + 6, new float3(nodeMid.x, nodeMid.y, nodeMin.z), new float3(nodeMax.x, nodeMax.y, nodeMid.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
if (p.z + radius > nodeMid.z)
{
QueryRec(ref query, childIndex + 7, new float3(nodeMid.x, nodeMid.y, nodeMid.z), new float3(nodeMax.x, nodeMax.y, nodeMax.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
}
}
}
else if (movementPlane == MovementPlane.XY)
{
if (p.x - radius < nodeMid.x)
{
if (p.y - radius < nodeMid.y)
{
QueryRec(ref query, childIndex + 0, new float3(nodeMin.x, nodeMin.y, nodeMin.z), new float3(nodeMid.x, nodeMid.y, nodeMax.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
if (p.y + radius > nodeMid.y)
{
QueryRec(ref query, childIndex + 1, new float3(nodeMin.x, nodeMid.y, nodeMin.z), new float3(nodeMid.x, nodeMax.y, nodeMax.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
}
if (p.x + radius > nodeMid.x)
{
if (p.y - radius < nodeMid.y)
{
QueryRec(ref query, childIndex + 2, new float3(nodeMid.x, nodeMin.y, nodeMin.z), new float3(nodeMax.x, nodeMid.y, nodeMax.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
if (p.y + radius > nodeMid.y)
{
QueryRec(ref query, childIndex + 3, new float3(nodeMid.x, nodeMid.y, nodeMin.z), new float3(nodeMax.x, nodeMax.y, nodeMax.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
}
}
else
{
if (p.x - radius < nodeMid.x)
{
if (p.z - radius < nodeMid.z)
{
QueryRec(ref query, childIndex + 0, new float3(nodeMin.x, nodeMin.y, nodeMin.z), new float3(nodeMid.x, nodeMax.y, nodeMid.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
if (p.z + radius > nodeMid.z)
{
QueryRec(ref query, childIndex + 1, new float3(nodeMin.x, nodeMin.y, nodeMid.z), new float3(nodeMid.x, nodeMax.y, nodeMax.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
}
if (p.x + radius > nodeMid.x)
{
if (p.z - radius < nodeMid.z)
{
QueryRec(ref query, childIndex + 2, new float3(nodeMid.x, nodeMin.y, nodeMin.z), new float3(nodeMax.x, nodeMax.y, nodeMid.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
if (p.z + radius > nodeMid.z)
{
QueryRec(ref query, childIndex + 3, new float3(nodeMid.x, nodeMin.y, nodeMid.z), new float3(nodeMax.x, nodeMax.y, nodeMax.z), ref maxRadius);
radius = math.min(radius, maxRadius);
}
}
}
}
}
