public void CompareNodes(int A, int B)
{
if (A >= 0 &&
B >= 0 &&
BVHArray[A].IsValid > 0 &&
BVHArray[B].IsValid > 0)
{
if (PhysicsMath.AABBToAABBOverlap(BVHArray[A].aabb, BVHArray[B].aabb))
{
// if leaf node
if (BVHArray[A].FirstChildIndex < 0)
{
CollisionPair newPair = new CollisionPair
{
ColliderEntityA = BVHArray[A].AssociatedEntity,
ColliderEntityB = BVHArray[B].AssociatedEntity,
};
CollisionPairsQueue.Enqueue(newPair);
}
else
{
NodePairsStack.Enqueue(new NodePair()
{
A = A, B = B
});
}
}
}
}
public void CompareNodes(int A, int B)
{
int childA1Index = BVHArray[A].FirstChildIndex;
int childA2Index = childA1Index + 1;
int childB1Index = BVHArray[B].FirstChildIndex;
int childB2Index = childB1Index + 1;
// always compare own children
if (BVHArray[A].IsValid > 0 &&
BVHArray[A].FirstChildIndex >= 0)
{
CompareNodes(childA1Index, childA2Index);
}
if (BVHArray[B].IsValid > 0 &&
BVHArray[B].FirstChildIndex >= 0)
{
CompareNodes(childB1Index, childB2Index);
}
if (BVHArray[A].IsValid > 0 &&
BVHArray[B].IsValid > 0)
{
if (PhysicsMath.AABBToAABBOverlap(BVHArray[A].aabb, BVHArray[B].aabb))
{
// if leaf node, add as collision pair
if (BVHArray[A].FirstChildIndex < 0)
{
CollisionPair newPair = new CollisionPair
{
ColliderEntityA = BVHArray[A].AssociatedEntity,
ColliderEntityB = BVHArray[B].AssociatedEntity,
};
CollisionPairsQueue.Enqueue(newPair);
}
// if not, compare their children
else
{
// Compare child nodes
CompareNodes(childA1Index, childB1Index);
CompareNodes(childA1Index, childB2Index);
CompareNodes(childA2Index, childB1Index);
CompareNodes(childA2Index, childB2Index);
}
}
}
}
void QueryBVHNode(int comparedToNode, int leafNodeIndex)
{
if (BVHRightmostLeafIndex[comparedToNode] > leafNodeIndex &&
BVHIsValid[comparedToNode] > 0 &&
PhysicsMath.AABBToAABBOverlap(BVHAABB[leafNodeIndex], BVHAABB[comparedToNode]))
{
// leaf node
if (BVHFirstChildIndex[comparedToNode] < 0)
{
CollisionPairsQueue.Enqueue(new CollisionPair
{
ColliderEntityA = BVHAssociatedEntity[leafNodeIndex],
ColliderEntityB = BVHAssociatedEntity[comparedToNode],
});
}
else
{
int firstChildIndex = BVHFirstChildIndex[comparedToNode];
QueryBVHNode(firstChildIndex, leafNodeIndex);
QueryBVHNode(firstChildIndex + 1, leafNodeIndex);
}
}
}
void QueryBVHNode(int comparedToNode, int leafNodeIndex)
{
if (BVHArray[comparedToNode].RightmostLeafIndex > leafNodeIndex &&
BVHArray[comparedToNode].IsValid > 0 &&
PhysicsMath.AABBToAABBOverlap(BVHArray[leafNodeIndex].aabb, BVHArray[comparedToNode].aabb))
{
// leaf node
if (BVHArray[comparedToNode].FirstChildIndex < 0)
{
CollisionPair newPair = new CollisionPair
{
ColliderEntityA = BVHArray[leafNodeIndex].AssociatedEntity,
ColliderEntityB = BVHArray[comparedToNode].AssociatedEntity,
};
CollisionPairsQueue.Enqueue(newPair);
}
else
{
int firstChildIndex = BVHArray[comparedToNode].FirstChildIndex;
QueryBVHNode(firstChildIndex, leafNodeIndex);
QueryBVHNode(firstChildIndex + 1, leafNodeIndex);
}
}
}