public void Execute()
{
int halfBVHArrayLength = BVHAABB.Length / 2;
// Populate leaf nodes
for (int i = 0; i < AABB.Length; i++)
{
int colliderIndex = sortResultsArrayIsA[0] == 1 ? indexConverterA[i] : indexConverterB[i];
int bvhIndex = halfBVHArrayLength + i;
BVHAABB[bvhIndex] = AABB[colliderIndex];
BVHAssociatedEntity[bvhIndex] = Entity[sortResultsArrayIsA[0] == 1 ? indexConverterA[i] : indexConverterB[i]];
BVHRightmostLeafIndex[bvhIndex] = bvhIndex;
BVHFirstChildIndex[bvhIndex] = -1;
BVHIsValid[bvhIndex] = 1;
BVHColliderType[bvhIndex] = ColliderType[colliderIndex].Value;
}
// Set all parent nodes to valid
int indexOfLastValidLeafOnLevel = halfBVHArrayLength + AABB.Length - 1;
while (indexOfLastValidLeafOnLevel > 0)
{
indexOfLastValidLeafOnLevel = PhysicsMath.GetParentBVHNodeIndex(indexOfLastValidLeafOnLevel);
if (indexOfLastValidLeafOnLevel == 0)
{
BVHIsValid[0] = 1;
}
else
{
for (int i = PhysicsMath.GetNextLowestPowerOf2(indexOfLastValidLeafOnLevel) - 1; i <= indexOfLastValidLeafOnLevel; i++)
{
BVHIsValid[i] = 1;
}
}
}
// Populate parent nodes bottom-up
for (int i = halfBVHArrayLength - 1; i >= 0; i--)
{
int childNode1 = PhysicsMath.GetFirstChildBVHNodeIndex(i);
int childNode2 = childNode1 + 1;
int nbNodesOnLevel = PhysicsMath.GetNextLowestPowerOf2(i + 1);
int nodeOrderOnLevel = i - nbNodesOnLevel + 2;
int leafNodesCount = (BVHAABB.Length + 1) / 2;
BVHFirstChildIndex[i] = PhysicsMath.GetFirstChildBVHNodeIndex(i);
BVHRightmostLeafIndex[i] = (BVHAABB.Length - 1) - ((leafNodesCount / nbNodesOnLevel) * (nbNodesOnLevel - nodeOrderOnLevel));
BVHAABB[i] = PhysicsMath.GetEncompassingAABB(BVHAABB[childNode1], BVHAABB[childNode2]);
}
}
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);
}
}
}
public void Execute()
{
// Calculate all morton codes and init sorted index map
for (int i = 0; i < aabbs.Length; i++)
{
mortonCodesA[i] = PhysicsMath.CalculateMortonCode(PhysicsMath.GetAABBCenter(aabbs[i]));
indexConverterA[i] = i;
indexConverterB[i] = i;
}
// Radix sort ascending
for (int bitPosition = 0; bitPosition < 31; bitPosition++)
{
bool isEvenIteration = math.mod(bitPosition, 2) == 0;
// init histogram counts
zeroesHistogramCounter = 0;
onesHistogramCounter = 0;
zeroesPrefixSum = 0;
onesPrefixSum = 0;
// Compute histograms and offsets
for (int i = 0; i < aabbs.Length; i++)
{
int bitVal = 0;
if (isEvenIteration)
{
bitVal = (mortonCodesA[i] & (1 << bitPosition)) >> bitPosition;
}
else
{
bitVal = (mortonCodesB[i] & (1 << bitPosition)) >> bitPosition;
}
radixSortBitValues[i] = bitVal;
if (bitVal == 0)
{
radixSortOffsets[i] = zeroesHistogramCounter;
zeroesHistogramCounter += 1;
}
else
{
radixSortOffsets[i] = onesHistogramCounter;
onesHistogramCounter += 1;
}
}
// calc prefix sum from histogram
zeroesPrefixSum = 0;
onesPrefixSum = zeroesHistogramCounter;
// Reorder array
for (int i = 0; i < aabbs.Length; i++)
{
int newIndex = 0;
if (radixSortBitValues[i] == 0)
{
newIndex = zeroesPrefixSum + radixSortOffsets[i];
}
else
{
newIndex = onesPrefixSum + radixSortOffsets[i];
}
if (isEvenIteration)
{
mortonCodesB[newIndex] = mortonCodesA[i];
indexConverterB[newIndex] = indexConverterA[i];
}
else
{
mortonCodesA[newIndex] = mortonCodesB[i];
indexConverterA[newIndex] = indexConverterB[i];
}
}
sortResultsArrayIsA[0] = isEvenIteration ? 0 : 1; // it's the A array only for odd number iterations
}
}
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);
}
