// TODO: make SIMD
public void DistanceQuery(DistanceQueryInput query, NativeList <int> results)
{
float maxDistanceSqrd = query.maxDistance * query.maxDistance;
var stack = stackalloc int[256];
stack[0] = rootIndex[0];
var top = 1;
while (top > 0)
{
var index = stack[--top];
var node = nodes[index];
if (!IntersectionUtils.IsInRange(node->box.LowerBound, node->box.UpperBound, query.origin, maxDistanceSqrd))
{
continue;
}
if (node->isLeaf)
{
if ((query.layerMask & node->leaf.layer) == 0 && DoDistanceQuery(query, ref node->leaf))
{
results.Add(index);
}
}
else
{
stack[top++] = node->child1;
stack[top++] = node->child2;
}
}
}
public bool DistanceQuery(NativeBVHTree.DistanceQueryInput query)
{
return(IntersectionUtils.IsInRange(LowerBound, UpperBound, query.origin, query.maxDistance));
}
public bool RayQuery(NativeBVHTree.Ray ray)
{
var invD = 1 / ray.direction;
return(IntersectionUtils.DoesOverlap(LowerBound, UpperBound, ref ray, invD));
}
public void RaycastQuery(Ray ray, NativeList <int> results)
{
ray.direction = math.normalize(ray.direction);
var invD = math.rcp(ray.direction);
if (nodes[rootIndex[0]]->isLeaf)
{
RayLeaf(ref this, rootIndex[0]);
return;
}
var stack = stackalloc int[256];
stack[0] = rootIndex[0];
var top = 1;
while (top > 0)
{
var index = stack[--top];
var node = nodes[index];
var child1 = nodes[node->child1];
var child2 = nodes[node->child2];
if (child1->isLeaf && IntersectionUtils.DoesOverlap(ref child1->box, ref ray, invD))
{
RayLeaf(ref this, node->child1);
}
if (child2->isLeaf && IntersectionUtils.DoesOverlap(ref child2->box, ref ray, invD))
{
RayLeaf(ref this, node->child2);
}
var result = node->grandchildrenAabbs.Raycast(ray, invD);
ProcessResult(ref this, 0, child1->child1);
ProcessResult(ref this, 1, child1->child2);
ProcessResult(ref this, 2, child2->child1);
ProcessResult(ref this, 3, child2->child2);
void ProcessResult(ref NativeBVHTree tree, int childId, int id)
{
if (result[childId])
{
var child = tree.nodes[id];
if (child->isLeaf)
{
RayLeaf(ref tree, id);
}
else
{
stack[top++] = id;
}
}
}
}
void RayLeaf(ref NativeBVHTree tree, int id)
{
var child = tree.nodes[id];
if ((ray.layerMask & child->leaf.layer) == 0 && tree.CastRay(ray, ref child->leaf))
{
results.Add(id);
}
}
}
