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