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);
}
}
}
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)
{
CollisionPair newPair = new CollisionPair
{
ColliderEntityA = BVHAssociatedEntity[leafNodeIndex],
ColliderEntityB = BVHAssociatedEntity[comparedToNode],
};
CollisionPairsQueue.Enqueue(newPair);
}
else
{
int firstChildIndex = BVHFirstChildIndex[comparedToNode];
QueryBVHNode(firstChildIndex, leafNodeIndex);
QueryBVHNode(firstChildIndex + 1, leafNodeIndex);
}
}
}
protected override JobHandle OnUpdate(JobHandle inputDeps)
{
int collidersCount = _colliderGroup.AABB.Length;
if (collidersCount <= 0)
{
return(inputDeps);
}
// Resize arrays if needed
// TODO: just make these NativeLists?
if (mortonCodesA.Length < collidersCount)
{
mortonCodesA.Dispose();
mortonCodesB.Dispose();
indexConverterB.Dispose();
indexConverterA.Dispose();
radixSortBitValues.Dispose();
radixSortOffsets.Dispose();
mortonCodesA = new NativeArray <int>(collidersCount, Allocator.Persistent);
mortonCodesB = new NativeArray <int>(collidersCount, Allocator.Persistent);
indexConverterB = new NativeArray <int>(collidersCount, Allocator.Persistent);
indexConverterA = new NativeArray <int>(collidersCount, Allocator.Persistent);
radixSortBitValues = new NativeArray <int>(collidersCount, Allocator.Persistent);
radixSortOffsets = new NativeArray <int>(collidersCount, Allocator.Persistent);
}
var sortMortonCodes = new ComputeAndSortMortonCodes
{
aabbs = _colliderGroup.AABB,
mortonCodesA = mortonCodesA,
mortonCodesB = mortonCodesB,
indexConverterA = indexConverterA,
indexConverterB = indexConverterB,
radixSortBitValues = radixSortBitValues,
radixSortOffsets = radixSortOffsets,
sortResultsArrayIsA = sortResultsArrayIsA,
}.Schedule(inputDeps);
// Debug sorted mortons
if (PhysicsSystem.Settings.ShowZOrderCurve)
{
sortMortonCodes.Complete();
bool indexConverterIsA = sortResultsArrayIsA[0] == 1;
for (int i = 0; i < _colliderGroup.AABB.Length - 1; i++)
{
float3 fromPoint;
float3 toPoint;
if (indexConverterIsA)
{
fromPoint = PhysicsMath.GetAABBCenter(_colliderGroup.AABB[indexConverterA[i]]);
toPoint = PhysicsMath.GetAABBCenter(_colliderGroup.AABB[indexConverterA[i + 1]]);
}
else
{
fromPoint = PhysicsMath.GetAABBCenter(_colliderGroup.AABB[indexConverterB[i]]);
toPoint = PhysicsMath.GetAABBCenter(_colliderGroup.AABB[indexConverterB[i + 1]]);
}
Debug.DrawLine(fromPoint, toPoint, Color.red);
}
}
// Build BVH
int requiredBVHLength = (PhysicsMath.GetNextHighestPowerOf2(math.ceil_pow2(collidersCount) + 1)) - 1;
BVHArray.Dispose();
BVHArray = new NativeArray <BVHNode>(requiredBVHLength, Allocator.TempJob);
var constructBVH = new ConstructBVH()
{
Entity = _colliderGroup.Entity,
AABB = _colliderGroup.AABB,
ColliderType = _colliderGroup.ColliderType,
BVHArray = BVHArray,
indexConverterA = indexConverterA,
indexConverterB = indexConverterB,
sortResultsArrayIsA = sortResultsArrayIsA,
}.Schedule(sortMortonCodes);
// Draw bvh second-to-last groups
if (PhysicsSystem.Settings.ShowBVHBounds)
{
constructBVH.Complete();
for (int i = BVHArray.Length / 4; i < BVHArray.Length / 2; i++)
{
if (BVHArray[i].IsValid > 0)
{
DebugUtils.DrawAABB(BVHArray[i].aabb, UnityEngine.Random.ColorHSV());
}
}
}
// Init pairs queue
SphereSphereCollisionPairsQueue.Clear();
JobHandle buildCollisionPairs = new JobHandle();
//Build Pairs job
{
BuildCollisionPairsParallel buildCollisionPairsJob = new BuildCollisionPairsParallel()
{
HalfBVHArrayLength = BVHArray.Length / 2,
BVHArray = BVHArray,
CollisionPairsQueue = SphereSphereCollisionPairsQueue,
};
buildCollisionPairs = buildCollisionPairsJob.Schedule(_colliderGroup.AABB.Length, PhysicsSystem.Settings.BroadphaseSystemBatchCount, constructBVH);
}
//{
// BuildCollisionPairsSingle buildCollisionPairsJob = new BuildCollisionPairsSingle()
// {
// StartIndex = BVHArray.Length / 2,
// IndexCount = _colliderGroup.AABB.Length,
// BVHArray = BVHArray,
// CollisionPairsQueue = SphereSphereCollisionPairsQueue,
// };
// buildCollisionPairs = buildCollisionPairsJob.Schedule(constructBVH);
//}
//{
// NativeQueue<NodePair> NodeStack = new NativeQueue<NodePair>(Allocator.TempJob);
// BuildCollisionPairsGroupStack buildCollisionPairsJob = new BuildCollisionPairsGroupStack()
// {
// BVHArray = BVHArray,
// NodePairsStack = NodeStack,
// CollisionPairsQueue = SphereSphereCollisionPairsQueue,
// };
// buildCollisionPairs = buildCollisionPairsJob.Schedule(constructBVH);
// buildCollisionPairs.Complete();
// NodeStack.Dispose();
//}
//{
// BuildCollisionPairsGroupRecursive buildCollisionPairsJob = new BuildCollisionPairsGroupRecursive()
// {
// BVHArray = BVHArray,
// CollisionPairsQueue = SphereSphereCollisionPairsQueue,
// };
// buildCollisionPairs = buildCollisionPairsJob.Schedule(constructBVH);
//}
// Init pairs array
buildCollisionPairs.Complete();
if (PhysicsSystem.SphereSphereCollisionPairsArray.IsCreated)
{
PhysicsSystem.SphereSphereCollisionPairsArray.Dispose();
}
PhysicsSystem.SphereSphereCollisionPairsArray = new NativeArray <CollisionPair>(SphereSphereCollisionPairsQueue.Count, Allocator.TempJob);
// Dequeue pairs into array
DequeueIntoArray <CollisionPair> dequeuePairsJob = new DequeueIntoArray <CollisionPair>()
{
InputQueue = SphereSphereCollisionPairsQueue,
OutputArray = PhysicsSystem.SphereSphereCollisionPairsArray,
};
JobHandle dequeueCollisionPairs = dequeuePairsJob.Schedule(buildCollisionPairs);
//// Debug CollisionPairsArray
if (PhysicsSystem.Settings.ShowCollisionPairs)
{
dequeueCollisionPairs.Complete();
for (int i = 0; i < PhysicsSystem.SphereSphereCollisionPairsArray.Length; i++)
{
Color randomCol = new Color(UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f), UnityEngine.Random.Range(0f, 1f));
CollisionPair pair = PhysicsSystem.SphereSphereCollisionPairsArray[i];
Debug.DrawLine(PhysicsMath.GetAABBCenter(AABBFromEntity[pair.ColliderEntityA]), PhysicsMath.GetAABBCenter(AABBFromEntity[pair.ColliderEntityB]), randomCol);
}
}
// Need to complete jobs here because counter will be read in the next system
dequeueCollisionPairs.Complete();
return(dequeueCollisionPairs);
}
