Skip to content

Fix min node queryray closest #14

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Ph0t0nX wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Fix min node queryray closest #14

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
30a70c1
BVH.cs - In BVHPrecomputeHierarchyJob.Execute(), changed the line "if…
Ph0t0nX May 17, 2026
6412f1f
GeometryUtils.cs - added a tEntry parameter to IntersectsRay() becaus…
Ph0t0nX May 17, 2026
f84d8da
BVH.cs - added QueryRayClosest() (two overloads) and QueryRayAny(). I…
Ph0t0nX May 17, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 10 additions & 7 deletions com.trove.common/Runtime/GeometryUtils.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -173,7 +173,7 @@ public bool Contains(float3 point)
}

[MethodImpl(MethodImplOptions.AggressiveInlining)]
public bool IntersectsRay(float3 rayOrigin, float3 rayDirectionNormalized, float rayLength)
public bool IntersectsRay(float3 rayOrigin, float3 rayDirectionNormalized, float rayLength, out float tEntry)
{
float tMin = 0.0f;
float tMax = float.MaxValue;
Expand All @@ -195,22 +195,25 @@ public bool IntersectsRay(float3 rayOrigin, float3 rayDirectionNormalized, float

if (tMin > tMax)
{
tEntry = 0f;
return false;
}
}

if (tMax < 0.0f)
{
tEntry = 0f;
return false;
}

float distance = tMin;
if (distance <= rayLength)
{
return true;
}
tEntry = tMin;
return tMin <= rayLength;
}

return false;
[MethodImpl(MethodImplOptions.AggressiveInlining)]
public bool IntersectsRay(float3 rayOrigin, float3 rayDirectionNormalized, float rayLength)
{
return IntersectsRay(rayOrigin, rayDirectionNormalized, rayLength, out _);
}
}

Expand Down
127 changes: 120 additions & 7 deletions com.trove.spatialqueries/Runtime/BVH.cs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -375,18 +375,122 @@ public unsafe bool QueryRay<TCollector>(float3 rayOrigin, float3 rayDirectionNor
where TCollector : unmanaged, IBVHQueryCollector<TNodeData>
{
collector.OnBeginQuery();

if (SortedNodes.Length < 1)
{

if (SortedNodes.Length < 1) {
return false;
}


Stack nodesStack = new Stack(256);
int* nodesStackPtr = stackalloc int[nodesStack.Capacity];
BVHNode* nodesPtr = SortedNodes.GetUnsafeReadOnlyPtr();
TNodeData* leafDataPtr = LeafNodeDatas.GetUnsafeReadOnlyPtr();
int leafNodesCount = LeafNodeDatas.Length;

nodesStack.PushLast(nodesStackPtr, SortedNodes.Length - 1); // start at root node;
while (nodesStack.PopLast(nodesStackPtr, out int nodeIndex)) {
BVHNode node = nodesPtr[nodeIndex];

if (!node.AABB.IntersectsRay(rayOrigin, rayDirectionNormalized, rayLength) || !node.IsValid())
continue;

if (nodeIndex < leafNodesCount) {
collector.AddNode(leafDataPtr[node.DataIndex]);
} else {
for (int i = 0; i < BVHUtils.NbLeavesPerNode; i++) {
nodesStack.PushLast(nodesStackPtr, node.DataIndex + i);
}
}
}

return collector.HasFoundResults();
}

// Perform a raycast and fetch only the closest entity (no collector required)
// This version also gives the closest distance.
public unsafe bool QueryRayClosest(float3 rayOrigin, float3 rayDirectionNormalized, float rayLength,
out TNodeData closestHit, out float closestDistance)
{
closestHit = default;
closestDistance = float.MaxValue;

if (SortedNodes.Length < 1) {
return false;
}

Stack nodesStack = new Stack(256);
int* nodesStackPtr = stackalloc int[nodesStack.Capacity];
BVHNode* nodesPtr = SortedNodes.GetUnsafeReadOnlyPtr();
TNodeData* leafDataPtr = LeafNodeDatas.GetUnsafeReadOnlyPtr();
int leafNodesCount = LeafNodeDatas.Length;

float currentMaxT = rayLength;
bool found = false;

nodesStack.PushLast(nodesStackPtr, SortedNodes.Length - 1);
while (nodesStack.PopLast(nodesStackPtr, out int nodeIndex)) {
BVHNode node = nodesPtr[nodeIndex];
if (!node.IsValid())
continue;
if (!node.AABB.IntersectsRay(rayOrigin, rayDirectionNormalized, currentMaxT, out float tEntry))
continue;
if (tEntry >= currentMaxT)
continue;

if (nodeIndex < leafNodesCount) {
currentMaxT = tEntry;
closestDistance = tEntry;
closestHit = leafDataPtr[node.DataIndex];
found = true;
} else {
// Ordered child traversal: push children in descending tEntry order so closest pops first
int childBase = node.DataIndex;
float4 ts = new float4(float.MaxValue);
int4 idxs = new int4(-1);
for (int i = 0; i < BVHUtils.NbLeavesPerNode; i++)
{
int ci = childBase + i;
BVHNode child = nodesPtr[ci];
if (!child.IsValid())
continue;
if (child.AABB.IntersectsRay(rayOrigin, rayDirectionNormalized, currentMaxT, out float t))
{
ts[i] = t;
idxs[i] = ci;
}
}
// Insertion sort, descending by t — smallest t ends up on top of stack
for (int a = 0; a < 4; a++)
{
for (int b = a + 1; b < 4; b++)
{
if (ts[b] > ts[a])
{
(ts[a], ts[b]) = (ts[b], ts[a]);
(idxs[a], idxs[b]) = (idxs[b], idxs[a]);
}
}
}
for (int i = 0; i < 4; i++)
{
if (idxs[i] >= 0) { nodesStack.PushLast(nodesStackPtr, idxs[i]); }
}
}
}

return found;
}

// Returns true if any node was hit (useful for line-of-sight tests).
public unsafe bool QueryRayAny(float3 rayOrigin, float3 rayDirectionNormalized, float rayLength)
{
if (SortedNodes.Length < 1)
return false;

Stack nodesStack = new Stack(256);
int* nodesStackPtr = stackalloc int[nodesStack.Capacity];
BVHNode* nodesPtr = SortedNodes.GetUnsafeReadOnlyPtr();
int leafNodesCount = LeafNodeDatas.Length;

nodesStack.PushLast(nodesStackPtr, SortedNodes.Length - 1); // start at root node;
while (nodesStack.PopLast(nodesStackPtr, out int nodeIndex))
{
Expand All @@ -397,7 +501,7 @@ public unsafe bool QueryRay<TCollector>(float3 rayOrigin, float3 rayDirectionNor

if (nodeIndex < leafNodesCount)
{
collector.AddNode(leafDataPtr[node.DataIndex]);
return true;
}
else
{
Expand All @@ -408,7 +512,15 @@ public unsafe bool QueryRay<TCollector>(float3 rayOrigin, float3 rayDirectionNor
}
}

return collector.HasFoundResults();
return false;
}


// Perform a raycast and fetch only the closest entity (no collector required)
// Use this version if you don't care about the distance.
public bool QueryRayClosest(float3 rayOrigin, float3 rayDirectionNormalized, float rayLength, out TNodeData closestHit)
{
return QueryRayClosest(rayOrigin, rayDirectionNormalized, rayLength, out closestHit, out _);
}

public bool QueryNearestNeighbor(float3 position, ref NearestNeighborResultCollector<TNodeData> collector,
Expand Down Expand Up @@ -1002,7 +1114,8 @@ public void Execute()
{
NodeLevelDatas.Clear();

if (SortedNodes.Length < BVHUtils.NbLeavesPerNode)
// Special case for length 1 and 0
if (SortedNodes.Length <= 1)
{
if (SortedNodes.Length > 0)
{
Expand Down